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ABSTRACT 



Extended Kalman filtering is applied as an extension of the Position Location Re- 
porting System (PLRS) to track a moving target in the XY plane. The application uses 
four sets of observables which correspond to inputs from a fused-sensor array where the 
sensors employed are acoustic, seismic, or radar. The nonlinearities to the Kalman filter 
occur through the measured observables which are: bearings to the target only, ranges 
to the target only, bearings and ranges to the target, and a Doppler-shifted frequency 
accompanied by the bearing to that frequency. The observables are nonlinear in their 
relationships to the Cartesian coordinate states of the filter. 

Filter error covariances are portrayed as error ellipsoids about the latest target esti- 
mate made by the filter. Rotation of the ellipsoids is accomplished to avoid the cross 
correlation of the coordinates. The ellipsoids employed are one standard of deviation in 
the rotated coordinate system and correspond to a constant of probability of target lo- 
cation about the latest Kalman target estimate. 

Filtering techniques are evaluated for both stationary and moving observers with 
arbitrarily moving targets. The objective of creating a user-friendly, personal computer 
based tracking algorithm is also discussed. 
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THESIS DISCLAIMER 

The reader is cautioned that the computer programs developed in this research may 
not have been exercised for all cases of interest. While every effort has been made within 
the time available to ensure that the programs are free of computational or logical er- 
rors, they cannot be considered validated. Any application of these programs without 
additional verification is at the risk of the user. 
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I. INTRODUCTION 



The military principle of combat maneuver, in its most basic sense, relies upon the 
knowledge of the ground commander as to his position and orientation within the com- 
bat arena. The units involved in a joint maneuver must coordinate their positions and 
orientations with each other in order to ensure maximum cooperation and the successful 
prosecution of the military campaign. Recent history dictates that the knowledge of a 
tactical military unit position has rested in the individual skill of members of the unit 
with a map and lensatic compass, from which a position could be determined. Orien- 
tation of the unit could be controlled through the accurate association of surrounding 
prominent terrain features to the map. Once location and orientation have been estab- 
lished, they must be transmitted to higher headquarters in order to ensure the proper 
adjacent unit coordination. This knowledge, along with tactical intelligence information 
about the position and orientation of local enemy forces allows the ground commander, 
through his staff, to command and coordinate support operations. These operations 
would include indirect fire support, close-air support, deep-air support, and logistics and 
communication support. 

The Position Location Reporting System (PLRS) was created by the Hughes Cor- 
poration of Los Angeles for the Marine Corps and Army to ensure, or at least improve, 
command knowledge of the positions and orientations of friendly tactical elements 
within the combat arena [Ref. 1]. This system, however, does not track enemy forces 
forward of the Forward Edge of the Battle Area (FEBA). This can be overcome by the 
use of an extended Kalman filter algorithm designed as the tracking element with inputs 
from a fused sensor array. The sensors employed would be restricted to four different 
types of observables: target bearings, target ranges, both target bearings and target 
ranges, or a Doppler-shifted frequency signal accompanied by the bearing to that signal. 
The Kalman filter algorithm would take the sensor inputs and through a conversion 
process, show observed and estimated target tracks accompanied by a predicted track 
based on the last estimated position of the target. An addition to the Kalman filter al- 
gorithm could then allow for the display of ellipsoids of constant probability (error 
ellipsoids) about the estimated target positions to show an area in which the target is 
located to a definite probability. 
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The operation of PLRS will be discussed, at least partially, in the next chapter. The 
accumulated knowledge of the PLRS observables (position and location of friendly 
units) and the observables of the algorithms developed in this thesis could contribute 
greatly to the knowledge that a potential ground tactical commander could gain about 
his situation within the combat arena and serve to aid in the tactical decision making 
process. The knowledge of friendly and enemy forces within a closed combat arena is 
essential to the successful prosecution of military objectives. This axiom is best summed 
up by the Chinese philosopher general, Sun Tzu [Ref. 2] who said 

If you know the enemy and know yourself, you need not fear the result of a hundred 
battles. If you know yourself but not the enemy, for every victory gained you will 
also suffer a defeat. If you know neither the enemv nor vourself, you will succumb 
in every battle .... 

One can easily see the necessity for accumulated combat maneuver intelligence. 
Since PLRS has no ability to track an enemy force which is forward of the FEB A and 
concentrates solely on friendly units, a potential tracking algorithm is developed in an 
attempt to augment the PLRS system. The algorithm is as previously described and 
based on multiple fused sensors providing measured observables to the tracking element 
for processing. Four types of observables were considered: target bearings only, target 
ranges only, both target bearings and target ranges, and a Doppler-shifted frequency 
accompanied by the bearing to that frequency. These observables are then fed to an ex- 
tended Kalman filter simulation algorithm to produce the estimated and the predicted 
tracks. 

The extended Kalman filter assumes the existence of nonlinearities between the ob- 
servables and the system states. The simulations conducted restrict movement of the 
target to the XY plane and the system states are derived from these dimensions. Non- 
linearities to the filter algorithms occur in all of the observables considered relative to 
the derived system states. An error ellipsoid algorithm is then employed in order to fur- 
ther isolate the target and to attempt to confirm its probable location. The algorithms 
are then combined using a personal computer-based, user-friendly driver routine in order 
to acquire the necessary inputs. The total simulation program is then analyzed for per- 
formance and an assessment of its tracking capabilities provided. 
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II. POSITION LOCATION REPORTING SYSTEM 



The PLRS system was developed for many different reasons — among them was the 
ability to protect friendly tactical elements from fire from friendly sources during combat 
situations and during peacetime training exercises. Accidents occur every year at the 
Marine Corps Air Ground Combat Center (MCAGCC), Twentynine Palms, California, 
which are a direct result of a lack of knowledge as to subordinate unit positions by 
command elements of friendly units. This is caused by untimely updating over command 
communication nets. The Combined Arms Exercise (CAX) and the Regimental Firing 
Exercise (FEX) are commonly used to combine actual movement of troops with live fire 
in order to impress upon commanders the importance of proper coordination and to 
increase realism in combat training. All of the movement corridors at MCAGCC are 
considered as live fire areas and, therefore, the positions of all maneuver elements within 
them is considered essential to both personnel safety and to mission accomplishment. 

The system consists of a containerized master unit composed of generalized com- 
puter, communications, and command electronics. This unit is designated as the Master 
Unit (MU). It is responsible for processing all information inputs into the system and 
for the display of unit positions, coordination measures, and control information visu- 
ally. Locally-placed user units are man or machine portable and are designated as User 
Units (UU). The MU sends a radio message to the UU which then computes a time of 
arrival (TOA) for the message and transmits this information back to the MU. The MU 
then uses the arrival time information from the UU to compute a range to the UU. The 
UU also transmits the local barometric pressure to the MU. The MU then uses the 
range, bearing, and pressure information to compute a three-dimensional position of the 
UU. Since the MU will not always have line-of-sight radio contact with all of the local 
user units, the system relies on the user units to determine the distances between them- 
selves and other locally-placed user units by computation of a TOA for the transmissions 
of the other units and to report this range information to the MU. Reports are made 
to the MU through a multi-level relay technique using other user units as necessary. 

Within PLRS, the location of each unit is established through a process called 
trilateration. This involves taking the ranges from three other units with previously es- 
tablished positions and using those ranges to calculate the position of the unlocated unit. 
To accomplish this the MU uses four simplified discrete Kalman filters. These filters are 
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called the Central Logic Oscillator Control (CLOC) filter to process clock offset and drift 
rate, the Mean Sea Level (MSL) filter to process an offset calibration for the barometric 
pressure measurements reported to it, the Track Review and Correction Estimation 
(TRACE) filter to enter and partially update each UU position and velocity estimate, 
and an altitude filter to process the barometric pressure data in order to establish and 
aid in tracking the vertical positions of the local user units. 

Tracking of the user units is accomplished by the ML' using an internal coordinate 
system. The internal coordinate system serves as the basis for all coordinate transfor- 
mation processing. This system is a transverse mercator projection with its central 
meridian at the longitude of system center. The system essentially takes the curved sur- 
face of the earth and projects it stereographically onto a flat plane. This is then called 
the Local Transverse Mercator (LTM) system. The LTM system produces a slight 
amount of distortion for long range measurements at distances far from system center. 
[Ref. 3] 

To use the LTM system, the TOA measurements must be converted to LTM ranges 
by PLRS in real time. This conversion is accomplished in four steps by the MU and is 
illustrated in Figure 1 on page 5. The four steps are listed as follows: 

1. The conversion of the TOA measurement to a true range measurement. 

2. The conceptual movement of the units to an average altitude of zero. 

3. The projection of the earth onto a fiat plane. 

4. The movement of the units back to their original altitudes. 

The use of PLRS greatly improves command and control efforts at the 
regimental brigade level and above. By provision of improved manuever unit movement 
updates and an increase in communications speed, the PLRS concept significantly im- 
proves fire support coordination and logistical support efforts. The improvements gained 
decrease personnel safety risks while allowing for realism in peacetime training, and in- 
creased unit effectiveness in combat. 
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Figure 1. MU Conversion of TOA Data to LTM Range: From {Ref. 3] 
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III. MULTIPLE GEOMETRY TARGET TRACKING ALGORITHM 



In order to study the problem by simulation, three different types of simulation 
models had to be developed. The sensors, when actually employed in the field, would 
track a real moving target across the ground. Since a real target does not exist, for the 
purposes of this simulation, a model had to be developed. The sensor system must also 
be modeled so that the observables developed from it can be fed to the extended Kalman 
filter algorithm for processing. The extended Kalman filter must be modeled in a com- 
puter algorithm so as to demonstrate its tracking capability. The simulations must then 
be iterated in order to demonstrate target movement and subsequent updating of the 
estimated positions and velocities of the modeled target. 

A. TARGET MODEL 

The motion of the target is restricted to the XY plane. The simulation model uses 
the basic equation of motion from kinematics 



5 = Sq + Jq T + — a $ T* (3.1) 

This relation is applied in both the X and Y directions and over the total observation 
time 



T total ^max x T (3-2) 

where k max is the number of target track positions desired. Thus, a complete set of target 
positions over a defined time period can be defined. These positions can then serve as 
the basis for the sensor and Kalman tracker algorithms. 

B. SYSTEM MODEL 

The system modeled is that of a ground target moving in the XY plane. Certain 
restrictions were placed on the problem in order to simplify the mathematics. These re- 
strictions were: 

• the curvature of the earth is neglected, 

• target and sensor movement are restricted to a flat plane, and 

• target course and speed inputs are constant (i.e., step inputs). 
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The observed matrix developed from the sensor simulation algorithm contains no noise, 
so that Gaussian noise can be added to the observations before entry into the extended 
Kalman filter algorithm. This supports realism in the simulation in that the observation 
measurements would contain Gaussian noise. 

This is a linear, time-distance system that can be described with equations of motion 
for constant acceleration in two dimensions. The state space equation is 

£k + 1 “ &k@k ( 3 - 3 ) 



where 

x,, = state vector (estimation parameter) 

0* = state transition matrix (describes how the dynamic system states are related) 

Ai = system noise coefficient matrix 
a k — random forcing function. 

When the above conditions and Equation (3.3) are incorporated, and the observable 
system is target bearings only, target ranges only, or both target bearings and target 
ranges, the state vector is 




(3.4) 



When the set of observables is the Doppler-shifted frequency measurement and the as 
sociated bearing to that frequency, then the state vector is 



.v 

x 



£k = 



y 

y 



fo 



(3.5) 



where f 0 is the target transmission rest frequency. 

When the following model equations are used and the aforementioned problem 
conditions incorporated into them, the system state equation can be expanded in matrix 
form. The model equations are 
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(3.6) 



x k+i 



~ x k + x kT + Y a xkJ l 



x k+\ = x k + a xk T 2 (3.7) 

Ta+i = >'k +hT + Y a yk^ 2 (3-8) 

S'k + 1 = fk ■*” °ykT 0 - 9 ) 

/o ( .,-/o, (3.10) 



Thus for the systems where the state vector is described by Equation (3.4), the system 



state equation can be expressed as 
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For the systems where the state vector is defined by Equation (3.5), the system state 
equation can be expressed as 
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(3.12) 



w T here f through f 5 are the random forcing functions included to account for random 
changes in speed, direction, or transmission frequency which can occur for a moving 
target. Each of these random forcing functions must be considered independently in or- 
der to ascertain their effect. The forcing functions can be expressed by 
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h =hiy e ,' ?»,. *) 

A =Mye,' Vv ,. k ) 



(3.15) 



(3.16) 

♦ 

fs =/ 5 (y /o ) (3-17) 

where y 9r (for heading), y U( (for speed), and y /o (for the transmission rest frequency) are 
considered as random changes to the target. These changes are assumed to be inde- 
pendent, zero mean, and piecewise constant rates of change. They have variances defined 
as 



G l t = £[(vv,) 2 ] 


(3.18) 


< = £[(>'*/] 


(3.19) 




(3.20) 



The system noise process for the target tracking and prediction problem is a function 
of the system noise coefficient matrix A*, and the random forcing function a k as defined 
for each of the state vectors. For the state vector of Equation (3.4) this is simply the 
target acceleration vector. For the state vector of Equation (3.5) a k is defined by 



= 



/. 

fl 

h 



A 

fs 



(3.21) 



The state space representation for the Doppler-shifted frequency observable was devel- 
oped by Mitschang [Ref. 4). 

C. MEASUREMENT MODEL 

The problem, as studied, considered four types of observables: bearings to the target 
only, ranges to the target only, both bearings and ranges to the target, and the 
Doppler-shifted frequency accompanied by a bearing measurement to that frequency. 
With recall of the state vectors as defined by Equations (3.4) and (3.5), the measurement 
model for this problem can be developed. The measurement equation is 
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Zk - Ulc^k + U* 



(3.22) 



where 

z k = the set of measurements 

if* = the observation matrix (noiseless) 

£ k = the state vector 

t>* = associated measurement noise. 

1. Bearings Only Problem 

In this tracking problem, the measurements are lines of bearing as received by 
sensors located on separate prominent terrain features. The geometry of the problem is 
shown in Figure 2 on page 11. The problem geometry shows that the relationship of 
the measurements to the system states is nonlinear. For this case the measurement 
equation becomes 



where 

Q nk = lines of bearing from sensor n at time k 
x tk y rk = the X.Y coordinates of the target at time k 
x nk y nk = the X.Y coordinates of sensor n at time k 
u* = the bearing measurement noise. 

2. Ranges Only Problem 

In this tracking problem, the measurements are straight-line distances to the 
target gained by ranging sensors located on separate prominent terrain features. The 
problem geometry is shown in Figure 3 on page 12. This geometry, as in the last case, 
shows a nonlinear relationship between the measurements and the system states. The 
measurement equation is 




( x tk ~ *„k) 
( Vik ~ }'nk ) 



(3.23) 




(3.24) 



where r nk = range distances from sensor n at time k. 
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Target 




2 



Figure 2. Bearings Only Geometry': Measurement Inputs to the Kalman Filter 

are Restricted to Target Bearings. 
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Figure 3. Ranges Only Geometry: Measurement Inputs to the Kalman Filter are 

Restricted to Target Ranges. 
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3. Bearings and Ranges Problem 

The tracking problem now becomes a combination of the first two problems. 
The measurements are defined as in Equation (3.23) for target bearings and Equation 
(3.24) for target ranges. The geometry for this problem is shown in Figure 4 on page 
14. The observation matrix for this problem is now defined as 



• z k = 



01 
r i 

02 
r 2 



(3.25) 



4. Doppler-Shifted Frequency Problem 

In this tracking problem, the observables are a Doppler-shifted frequency ac- 
companied by a bearing measurement to that frequency. The problem geometry as 
shown in Figure 5 on page 15, along with knowledge of the Doppler equation [Ref. 5], 
shows that the relationship between the measurements and the system states is again 
nonlinear. The measurement equation is 



tan 



-iT x ,k ~ x nk I 

L -Wfc J'nllr J 

. Mk)v p 



v p + 



/ 2 , T 

V x ik ' y'tk 



+ 



L 1 )J 



(3.26) 



where 

v p = the velocity of wave propagation (speed of light) 
v ek = bearing measurement noise 
v* = frequency measurement noise. 

This measurement equation is based on a single sensor which is stationary 7 and located 
at the origin. 

The observables in all four measurement cases have been shown to be nonlinear 
in their relationship to the system states. Processing by the extended Kalman filter is 
thus required for target tracking and prediction. The operation of the extended Kalman 
filter will be explained in the next chapter. 
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rigure 4. Bearings and Ranges Geometry: Measurement Inputs to the Kalman 

Filter are Restricted to Target Bearings and Target Ranges. 
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Target 




Obseruer 

1 



Figure 5. Doppler Frequency Geometry: Measurement Inputs to the Kalman 

Filter are Restricted to a Doppler Shifted Frequency and the Associated 
Frequency Bearing. 
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IV. KALMAN FILTER THEORY 



The process of estimation of the state vector at the current time, with reference to 
all previous measurements, is referred to as filtering. An optimal filter optimizes a spe- 
cific performance measurement which is used to approximate the quality of the estimate. 
The Kalman filter is a linear optimal filter which minimizes the mean square estimation 
error between the actual output and the desired output. The filter, in actuality, is a re- 
cursive algorithm for the optimal processing of discrete measurements or observations 
[Ref. 6]. The filter requires an a priori knowledge of the state estimate and its error 
covariance as well as the current observation. System equations for the Kalman filter 
are 



&k + 1 “ 



(4.1) 



and 



ik = EkXk + 12 * 



(4.2) 



A. THE EXTENDED KALMAN FILTER 

From the measurement equations in the last chapter for each of the observable 
cases, one can see that there is a nonlinear relationship between the measurements used 
for target tracking and the system state variables. The adaptation of the Kalman filter 
to a nonlinear application is termed as the extended Kalman filter. A general discussion 
of the of this type of filter algorithm follows. 

Given system and measurement equations of the form 

x k k) + g(x k , k)w k (4.3) 

ik = hUkt k ) + z k (4.4) 

where 

f, £, h are nonlinear functions of the state vector x , 
w* is the plant excitation noise, 
v* is the measurement noise. 
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The plant excitation noise and the measurement noise are assumed as uncorrelated, zero 
mean, and white. That is 



w k = N(0 ,Q), (4.5) 

ft = iV(0, R), (4.6) 

and 

w/] = Qk^kJ (4-7) 

£[fti i/] = Rk^kj (4.S) 

where 6 kj is the Kronecker delta function such that 

hj= 1 (* =j) (4.9) 

S kJ = 0 (k*j). (4.10) 

To apply the linear filter Equations (4.1) and (4.2), an expansion of Equations (4.3) 
and (4.4) is conducted about the best estimate of the state at the current time. This ex- 
pansion is accomplished with a Taylor series and only the first order terms are kept. 
With the expansion. Equation (4.3) now yields 

Jfc-M =£*£* + &k&k ( 4M ) 



where 



2 






cx k 



£* = ** 



(4.12) 



Equation (4.4) now yields 



Z k — -tlk—k 4* Ik 



(4.13) 



where 







(4.14) 



17 



The variable i* is the estimated value of the state after the k ,h measurement and the 
variable xl* is the predicted state value before the k' h measurement. These can be speci- 
fied more completely by 

xL k =Mk-i,k-l)- ( 4 - 15 ) 

The following set of definitions can then be used to derive the linear Kalman filter 
equations used in the simulation. A state error vector can be defined as 

£ k = £ k -£ k . (4.16) 

A predicted state error vector can then be defined by 

£' k = t k -£ k - (4.17) 

Equations (4.16) and (4.17) allow the definition of the covariance of state error matrix 
& by 

£*=£[mF] 

and the definition of the predicted covariance of state error matrix P' k by 

£* = £[ 2 * 21 ]. (-119) 

The state excitation matrix Q k , as seen in Equation (4.7), is defined by 

Q k = EL&k&kPk&k]- ( 4 - 2 °) 

The measurement noise covariance matrix R k , as seen in Equation (4.8), is defined by 

(4-21) 

The definitions cited above provide the basis for the construct for the Kalman filter al- 
gorithm specified by the set of equations below [Ref. 6] 



£*+ 1 — + Qk 


(4.22) 


a k -Ek^uikEka T k +B.kr' 


(4.23) 


&J 
1 — 1 

1 

>■— 

1 1 

II 

a? 


(4.24) 
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* 





(4.25) 


zL k = h(xL k , A) 


(4.26) 


£ k = £i k + - zlj 


(4.27) 



where 

I = the identity matrix 

(2* = the Kalman gain matrix. 

The Q matrix allows for target maneuvering and also accounts for filter model in- 
accuracies. This greatly decreases the discrepancies which would arise between the sys- 
tem characterization, Equations (4.1) and (4.2), and the true action of the system 
physically. As the filter algorithm reaches steady-state conditions, the Q matrix also 
prevents the Kalman gain matrix, , from approaching zero by ensuring an amount 
of uncertainty in the predicted covariance of the state error matrix £1* 

B. LINEARIZATION OF THE OBSERVATION MATRIX 

For purposes of brevity, the H matrix derivations will be done simultaneously for 
the bearings only, ranges only, and the bearings and ranges observable cases. These are 
slightly different, however they are based upon the same mathematical premise. 
Equations (4.4), (4.13), and (4.14) establish the basis for the linearization of the obser- 
vation matrix H. These equations state that the II matrix is a linearization of the non- 
linear function h and is achieved by taking the partial derivative of h with respect to the 
predicted state. 

1. Target-Bearing Measurement Observables Only 

The h function using bearing measurements only can be deduced from Equation 

(3.23) as 



*<*•*> -“"l 3^- ]■ 



Applying the above linearization method, 



E k = 



. -i 


(■*7 k x nk) 


tan 


( Vt k — y'nk) J 



d £k 



(4.28) 



(4.29) 
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where £ k can be recalled for Equation (3.4). Simplifying Equation (4.24) yields 



where 



//, 



^11 ^12 ^13 ^14 



I /i 2 ] h 22 h 2 3 h 2 4 



Af.1 = 



tan 



-if" (*,* ~ 

L (**- 



~X nk ) 



y'nk ) 



y'lk ~ y'nk 



dx, 



tk 



R 



nk 



tan 



-if* (*tk ~ -f 
L C Vtk-y, 



y'nk) 



l n 3 



n n 2 “ 


cx lk 


— VJ 












tan 


(■ x ,k ~ Xnk) 




c 


(y’tk-ynk) 


- (x,k - Xnk) 






d y)k 


Rn„ 



K 4 = 



„ -1 




tan 


O' “ vWjfc) 



O',* 



= o. 



From Equations (4.31 ) through (4.34) 



h n = 



— 3/ifc 



£ 2 






/j 12 — 0 



*>3 = 



~ (•*,* - x nk ) 



R, 



■nk 



*14 = 0 



(4.30) 

(4.31) 

(4.32) 

(4.33) 

(4.34) 

(4.35) 

(4.36) 

(4.37) 

(4.38) 



20 



where the range estimate squared R 2 nk is 



R-nk ( X i{k, k-\) x nk ) + (}'r(k, k-\) 3 'nk ) • 



(4.39) 



The second row of the matrix is completed in a similar manner. 

2. Target-Range Measurement Observables Only 

The target-range-measurement-only h function can be deduced from Equation 
(3.24) as 



h ( x h k ) = J( x ,k - x nk) + iy,k - y»k ) • 



(4.40) 



Applying the linearization method again yields 



Ek = 



*|>* x nk) + i}'tk y'nld J 

cx k 



(4.41) 



where again, x , is recalled from Equation (3.4). Simplification of Equation (4.36) yields 



H k = 



/(,, h n h x 3 h Xi 

k 2\ k 22 k 23 k 24 



(4.42) 



where 



K\ = 



5 [v’(*r* - x nk) 2 + Or* ->’nk) 2 ] 



}'ik }'nk 



CX x 



tk 



R 



nk 



(4.43) 



l n2 



c\_J( x ,k ~ x nk) 2 + Or* —}'nk) 2 ] 

5x lk 



= 0 



(4.44) 



Ki = 



( x lk x nk ) O';* }’nk) J ~( x tk~ x nk ) 






tk 



R 2 

K nk 



(4.45) 



K 4 = 



?[j( x tk - x nk ) 2 + 0’/* -3V) 2 ] 

6 >'tk 



= 0. 



(4.46) 



Thus from Equations (4.43) through (4.46) 
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x ,k - x nk 



(4.47) 



*12 = 0 (4.48) 

*13 = (4-49) 

*»* 

*, 4 = 0 (4.50) 

and as before, the second row can be computed in a similar manner. 

3. Target-Bearing and Range Measurement Observables 

The h functions for the case where the observables are both the target bearings 
and the target ranges are the same as those derived in both of the previous cases. These 
cases, however, are now combined. The corresponding H matrix is 



i4 = 



*11 *12 *13 *14 
*21 *22 *23 *24 
*3 1 *32 *33 *34 
*41 *42 *43 *44 



(4.51) 



where the first and third row of the matrix are the first and second rows of the matrix 
defined in Equation (4.30) replaced in a one-for-one correspondence. The second and 
fourth rows of the matrix are the first and second rows of the matrix defined in Equation 
(4.42) replaced one-for-one. 

4. Doppler-Shifted Frequency Observable 

The h function for the Doppler-shifted frequency observable and the associated 
bearing is somewhat more detailed. Using the state vector defined by Equation (3.5) and 
the measurements obtained from Equation (3.26), one can deduce the following shifted 
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Doppler h function 



h{x k , k) = 



A 

fk 



L J 




fo( k ) v p 


l > + 


x tk*{k yWik 


. Jxl+yi _ 



(4.52) 



Application of the linearization process yields 



fftk- 



k \\ ^12 ^13 ^14 ^15 
/l 21 /; 22 ^23 A 24 ^25 



where using the notation of Equation (4.15) 



c6 1 



-y tk 



h \ i — , — ■) 

cx ik x'-.u 4- v 



tk " r y tk 



h 12 = 



Sx lk 



= 0 



/i 13 — 



cd L 



X tk 



c y'tk x’- !k +y: k 



cO k 

/iu = -r jL = 0 



cytk 



8d k 



sjk _ - fly' tkly' ,kx' ,k - x' t ,y' lk ] 
dx>k f'ok v pUx’ 2 tk +y' 2 k) 2i2 l 

*fk _ -fi x 'tk 
' 22 8i * f'ok^(x' 2 k+y 2 tkf l2 i 



(4.53) 



(4.54) 



(4.55) 



(4.56) 



(4.57) 



(4.5S) 



(4.59) 



(4.60) 
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(4.61) 



cfk _ ~ f'\ x ' tki x ' rkS'' tk —y'tlcX'tk] 
*y* ~ fok^lk+y^i 



Vk —f'\y'tk 

”/w>c(^?*+y?*) ,/2 ] 



h 25 = 



Vk A 



v r 



Ok 



(4.62) 



(4.63) 



Having linearized the observation matrices for all of the observable cases, the linear 
Kalman filter equations can be utilized. 

C. ACQUIRED NOISE PROCESSES (R AND Q MATRICES) 

Calculation of the covariance of state error matrix, P k , and the Kalman gain matrix 
Gi, requires the specification of covariance matrices for the associated uncorrelated noise 
processes a k and v* as used in this study. The covariance matrix for the measurement 
noise process v k is found in Equation (4.8). R k is defined as the state measurement noise 
covariance matrix. This matrix is based on the accuracy of the sensors employed and 
accounts for unknown disturbances in the plant model. 

The state excitation matrix Q k represents the system noise process and is a function 
of the system noise coefficient matrix & k and the random forcing function a k . . Thus 

Qk - fA21,A r ] (4.64) 



where Q\ , from Equation (4.20), is defined as 



QLk = “k&h = 



£(<4) 

E^xk^k) 



E( a yk a xk) 

E( a >k) 



(4.63) 



This matrix is valid for the bearings and ranges observables and allows for any random 
target maneuvers and any inaccuracies in the system model. 

The derivation of the Q matrix for the Doppler observable is slightly more complex 
however, and is based on the random disturbances /, through / as defined by Equation 
(3.21). Recall that y 9( , y Vf , and y /Q were the random changes to the target and were as- 
sumed as independent, zero mean, and piecewise continuous. The variances for these 
changes can be defined as 
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(4.66) 



4 

4 , -£[’ 4 ,] (<'•«) 

<’/„-£[£] (4.68) 

. The standard deviations a, r and a, specify typical maneuvering parameters for the 
target. The Q matrix is found by letting 




a xy x ! 4 




(4.69) 

(4.70) 



(4.71) 



where the states are evaluated at the current state estimates jr*. Substitution of the above 
expressions into the Q matrix yields 



' 2 /2)M 


(r 3 /2)<4 


(T 2 l2)oly 


(7°/2)4 


?12 


(T 2 )o 2 x 


(T 3 l2)o 2 xy 


(T 2 )°ly 


<713 


q 23 


(T 2 l2fo 2 y 


(T*l2)oj 


<714 


<724 


q 34 


(T 2 )o 2 


<715 


<?25 


<735 


q 45 



(4.72) 



This analysis was done by Mitschang and is found in detailed form in Reference 4. 

D. PROBLEM PARAMETERS 

The following parameters were used in this analysis: 

<7 #f = 6.283 rad/hr 
a,, = 0.01852 km/hr 
a\ t =0.01096 (rad/min) 3 
a\ t =0.0001852 (Ion/ min 3 ) 3 . 

For the Doppler observable, the parameters were: 
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ey = 0.00017452 rad/sec 
er, r = 0.01852 km/ sec 
= 0.0005 Hz/sec. 

With the </> matrix defined by the system considered as in Chapter III, and the Q, R, and 
H matrices defined as above, the Kalman filter Equations (4.22) through (4.27) can be 
employed and the simulations conducted. 
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V. ERROR ELLIPSOIDS 



The position of each unit within the PLRS system is estimated with a certain finite 
degree of uncertainty about the estimate. The associated uncertainty is expressed in the 
covariance of error matrix P k . This represents the sigma squared error deviation about 
the estimate. The position diagonal terms, /*,, and P 33 , represent the variances of the es- 
timate in the X and Y directions respectively. The off-diagonal terms of each represent 
covariances, the degree of coordinate coupling, and the orientation of the uncertainty in 
the XY plane. 

The errors are normally distributed and there exists a rotated coordinate system in 
which the orthogonal position components are uncorrelated. This is identical to taking 
the exponent of the joint probability density function and performing a coordinate 
transformation to eliminate the cross terms. 

The exponent for Gaussian random variables is 



-t 2 xy 

2 lr xy o x a y 



(5.1) 



where r, y is the cross correlation of X and Y. When this expression is set equal to a 
constant, the geometric interpretation is that of an ellipse which specifies a constant 
probability of target location about the estimate. The major and minor axis of the ellipse 
are oriented in the XY coordinate system. In this system a cross correlation between X 
and Y exists. To eliminate this cross correlation, a coordinate transformation is applied. 
The positional component X is transformed to a new component X' and the positional 
component Y is transformed to a new component Y' by 

jc' = .* cos 9 +y sin 9 (5.2) 



and 



x' — y cos 9 — x sin 9 



(5.3) 



where 



n 1 t .-i r 2cov(^v) 

0 - . tan [ 2 2 J* 



a r — g u 



(5.4) 
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This specifies the variances in the new coordinate system (X'Y ) as 



2 “x ^ u v _ co vyry 
^ = 2 + sin W 



cov(.ty) 



(5.5) 



and 




(5.6) 



a >' 2 sin 26 - 



The relation used to plot the resulting ellipsoid, which defines a constant probability of 
target location about the latest Kalman estimate, results from the transformation. The 
relation is based on Equation (5.1) and is 



where K defines the probability of target location. 

The error ellipsoid routine employed in this study is based on the above premise and 
plots ellipsoids respresenting a constant probability of target location about the latest 
Kalman estimate and uses that estimate as the ellipsoid center. The program was written 
by Captain Stephen L. Spehn USMC in collaboration with another project. This pro- 
gram plots a single standard of deviation (in the orthogonal coordinates) ellipsoid with 
a sixty percent probability of target location within the ellipsoid. The major and minor 
axis of the ellipse may also be plotted in terms of the Mahalanobis distance or the dis- 
tance in standards of deviation [Ref. 7], The ellipsoids are increased in size by factors of 
as much as sixteen for ease of view. 




(5.7) 
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VI. PROGRAM RESULTS 



A. PROGRAM LOGIC 

The tracking problem solution integrates all four types of geometric concepts of 
Chapter III. In order to simulate the problem and to attempt a solution, each of the 
geometric concepts required an individual working subroutine. The subroutines were 
then integrated using a menu-driven, user-interactive driver file. The driver file was de- 
signed to acquire target position and movement information, tracking problem parame- 
ters, and to determine the type of problem to be solved. 

The program presented herein is an attempt to solve the geometric problem of 
tracking an arbitrarily moving target in the XY plane. It was written in PC-Matlab in 
order to use a personal computer-based mathematical algorithm. The system, when 
augmented and improved with combinational logic and mapping data, could be used by 
a maneuver commander in the combat arena as an additional tool in his assessment of 
the tactical situation. The advantage that could be gained in the knowledge of enemy 
position and intent would greatly aid in the tactical decision-making process. 

1. PC-Matlab Structure 

The personal computer-based Matlab program uses a unique program structure 
that is similar to Fortran. The program, however, uses different terminology in its 
structure when employing driver routines. The difference in terminology is derived from 
the way the main Matlab driver programs are written. All Matlab execution files are 
named with a .m suffix. Thus, all of the file names for this problem contain that suffix. 
The driver file, marine. m, calls the various subroutines as needed when in operation. The 
programs written for the study of this problem are included in the Appendix. They are 
commented throughout in attempt to help the reader to become familiar with their 
construct with as little effort as possible. 

2. Subroutine Structure 

The program subroutines each isolate one geometric concept in an attempt to 
simulate and solve the tracking problem. Each subroutine is written to simulate the tar- 
get, to simulate the observers (sensors), and to simulate the operation of the extended 
Kalman filter discussed in Chapter IV. The various steps are commented throughout in 
an effort to aid the reader in understanding the program and its structure. The names 
of the variables used in the programs of the Appendix correspond to those of Chapters 
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Ill and IV as much as possible. It was felt that this would aid in the understanding of 
its operation. 

B. RESULTS 

The results of the program discussed are presented graphically in order to illustrate 
what the maneuver commander would see if such a system were to be employed on the 
battlefield. Since the program tracks an arbitrarily moving target with arbitrary observer 
placement, the target tracks and the observer (sensor) locations were changed with each 
geometric case. The cases will be presented in the following order: target-bearing ob- 
servables only employed, target-range observables only employed, both target-bearing 
and range observables employed, and the Doppler observable with its associated bearing 
employed. 
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1. Target-Bearing Observables Only 

Results of a tracking problem using only the observed bearings from a fused- 
sensor array are contained in Figure 6 on page 32 through Figure 9 on page 35. These 
figures show outputs at various stages of the program. Figure 6 on page 32 shows the 
actual track of the target, the circles on the diagram, and the noiseless observations, the 
dots. This graph allows for a comparison of the bearing calculations by the program 
against the track as chosen in the simulation. This is necessary as a check in the opera- 
tion of the bearings only subroutine. If the comparison is successful, white noise is added 
to the bearing measurements before they are submitted to the Kalman algorithm. 
Figure 7 on page 33 shows the actual and estimated target tracks as well as the error 
ellipsoids around the Kalman estimates. The simulation iterates and, when in display on 
the screen, one can see the target move. In this case, the movement is from the upper 
left to the lower right. The rotation of the error ellipsiods to avoid coordinate cross 
correlation is clearly evident. The actual positions are again circles, the estimates are 
crosses, and the error ellipsoids are evident from the plot. Figure S on page 34 shows the 
actual, estimated, and predicted positions of the target. The predictions were obtained 
by taking the predicted Kalman state estimate and the current Kalman state estimate 
and performing a subtraction of the velocity values in the X and V directions and di- 
viding by the observation interval. This produced an estimate of the random forcing 
function a k as defined in Chapter III. The estimate was then used in the linear Kalman 
system equation shown in Equation (4.1) to determine predicted positions based on a 
constant acceleration. This is consistent with a second-order Kalman analysis in each 
direction where the acceleration is the random forcing function. The predicted positions 
are shown as stars in the diagram. Figure 9 on page 35 shows what the potential combat 
commander would see should the system be employed. The figure is a combination of 
the actual, estimated, and predicted target tracks as well as the error ellipsoids about the 
Kalman estimates. The figure also shows the location of the observers, as do the previ- 
ous figures. These are portrayed as an x in the diagram. Essentially the program was 
constructed to produce results in response to questions a potential maneuver 
commander would ask while engaged in combat operations. These questions are: 

• Where is the enemy ? 

• Where is he going ? 
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Tigure 6. Actual and Observed Tracks: Target-Bearing Observables Only. Ac- 

tual Track - o, Observed Track - • , Sensor - x. 
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Figure 7. Actual and Estimated Tracks: Target-Bearing Observables Only. Ac- 
tual Track - o, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Tigure 8. Actual, Estimated, and Predicted Tracks: Target-Bearing Observables 

Only. Actual Track - o, Estimated Track - + , Predicted Track - *. 
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Figure 9. Total Target Track View: Target-Bearing Observables Only. Actual 
Track - o, Observed Track - ♦ , Sensor - x. Predicted Track - *. 
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2. Target- Range Observables Only 

The results of a tracking problem using only target-range observables are shown 
in Figure 10 on page 37 through Figure 13 on page 40. These Figures are in the same 
order as the ones in the bearings only case. The target track and observer locations have 
been changed in order to demonstrate the versatility of the program. Figure 10 on page 
37 shows the noiseless observations compared to the actual track of the target as chosen 
in the simulation. Again, if the comparison is successful, Gaussian noise is added before 
filtering. Figure 1 1 on page 38 shows the actual and estimated tracks of the target as 
well as the error ellipsoids about the Kalman estimates. Figure 12 on page 39 shows the 
actual, estimated and predicted target tracks. When the display is on screen, one can 
observe the target movement, which in this case would be from left to right across the 
diagram. Since the observers are nearly colinear with the first estimate, the error ellipsoid 
shows a substantial bearing error while showing a small range error. This decreases as 
the target moves away from the observers because the range directions are more accu- 
rately determined. Figure 13 on page 40 is a combination of the previous figures which 
would be used to provide information about the enemy for use in the tactical decision 
making process. 

3. Target-Bearing and Range Observables 

The results of a problem employing both target-bearing and range observables 
. are shown in Figure 14 on page 41 through Figure 17 on page 44. The figures are again 
in the same order as the previous two cases to allow comparison. Figure 14 on page 41 
shows the actual target track and the noiseless observations for this case. If the obser- 
vations favorably compare with the actual positions, white noise is added and the noisy 
observations filtered by the Kalman algorithm. Figure 15 on page 42 shows the actual 
and estimated tracks with the error ellipsoids added. The rotation of the ellipsoids to 
avoid cross correlation of the coordinate variables is particularly evident in this case. 
Figure 16 on page 43 shows the actual, estimated, and predicted tracks for the target. 
The target movement in this case was from the lower right to the upper left. Figure 17 
on page 44 is a combination of the previous three and would be the result of the program 
when viewed in the field. Again, it provides answers to relevant command questions. 
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Actual and Observed Tracks: Target-Range Observables Only. Ac 

tual Track - o, Observed Track - • , Sensor - x. 
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Figure 11. Actual and Estimated Tracks: Target-Range Observables Only. Ac- 
tual Track - o, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Figure 12. Actual, Estimated, and Predicted Tracks: Target-Range Observables 
Only. Actual Track - o, Estimated Track - 4- , Predicted Track - *. 
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Figure 13. Total Target Track View: Target-Range Observables Only. Actual 

Track - o, Observed Track - • , Sensor - x. Predicted Track - *. 
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Figure 14. Actual and Observed Tracks: Target-Bearing and Range Observables. 
Actual Track - o, Observed Track - • , Sensor - x. 
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Figure 15. Actual and Estimated Tracks: Target-Bearing and Range Observables. 

Actual Track - o, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Figure 16. Actual, Estimated, and Predicted Tracks: Target«-Bearing and Range 

Observables. Actual Track - o, Estimated Track - +, Predicted Track 
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Figure 17. Total Target Track View: Target-Bearing and Range Observables. 

Actual Track - o, Observed Track - • , Sensor - x. Predicted Track - 

* 
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4. Doppler-Frequency Observable 

The results of a problem where a Doppler-shifted frequency and the associated 
bearing to that frequency are employed are shown in Figure 18 on page 46 through 
Figure 21 on page 49. These are based on a system employing one sensor, which is sta- 
tionary and located at the origin. Figure 18 on page 46 show r s the actual track positions 
compared to the target observations. If the observations compare to the actual posi- 
tions, white noise is added and the observables sent to the Kalman algorithm for proc- 
essing. Figure 19 on page 47 shows the actual and estimated tracks with the error 
ellipsoids to aid in defining target position. Figure 20 on page 48 shows the actual, es- 
timated , and predicted target tracks. Figure 21 on page 49 is again a combination of the 
previous three graphs and would be the one seen in the field. The target movement in 
this case was from upper left to lower right. 
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Figure 18. Actual and Observed Tracks: Doppler Frequency Observable. Actual 

Track - o, Observed Track - • , Sensor - x. 
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Figure 19. Actual and Estimated Tracks: Doppler Frequency Observable. Ac- 

tual Track - o, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Figure 20. Actual, Estimated, and Predicted Tracks: Doppler Frequency Ob- 

servable. Actual Track - o, Estimated Track - +, Predicted Track - *. 
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Figure 21. Total Target Track View: Doppler Frequency Observable. Actual 

Track - o, Observed Track - • , Sensor - x. Predicted Track - *. 
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VII. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

The target tracking algorithm developed here is both personal computer-based and 
user-friendly. This would aid in field employment where the system would be of most 
use to the maneuver commander. With improvements in the tracking algorithm pre- 
sented, or on one similiarly based, real-time tracking could be accomplished. These im- 
provements will be subsequently discussed. With real-time tracking, indirect supporting 
arms or direct air could engage the target with a reasonable chance of success. If the 
orientation of the target were known, optimal damage could then be inflicted upon it. 
Hence, the development of a working fused-sensor system tracking algorithm would 
place forward reconnaissance personnel at intelligent risk when seeking combat maneu- 
ver intelligence far forward of the FEBA. 

B. FUTURE STUDY POSSIBILITIES 

Future study regarding improvements in the algorithm developed is quite extensive. 
Topics which would facilitate the program's usefulness include incorporation of the ac- 
celeration terms in the X and Y directions and the application of the Z coordinate and 
its velocity and acceleration terms. PLRS uses time division multiple access (TDY1A) for 
unit communications transmissions. A delay study should be done so as to account for 
a worst-case analysis of the effect that transmission delays would have on the employ- 
ment of the Kalman tracker or its target update capability. 

C. PROGRAM IMPROVEMENTS 

Improved program structure can be achieved by employment of the principles used 
by Baheti (Ref. 8]. The Baheti algorithm requires one quarter of the memory' capacity 
and fewer computations than the algorithm as developed here, but does not adhere to 
conventional control methods. The algorithm herein presented was written in PC Matlab 
which is a matrix manipulation program. It will require updating as improvements in the 
.Matlab algorithm are developed. 

Since optimal tracking of a ground target would be futile in the fact that the ground 
target motion would be unpredictable, another possible program improvement could be 
researched. The Defense Mapping Agency has worldwide computer mapping data which 
can be displayed on a computer terminal. It may be possible to incorporate this data in 
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a logic algorithm structure so as to determine the target's tendencies to augment the 
optimal tracking scenario. Once the logical choices are made, the potential maneuver 
commander could employ logic as well as optimal control to thwart his adversary. 
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APPENDIX TARGET TRACKING SIMULATION PROGRAMS 



A. PROGRAM DISCUSSION 

The programs contained herein were written to simulate the various aspects of the 
target tracking problem heretofore discussed. They are written in PC-Matlab and hence, 
are denoted by a .m suffix to identify them as control or execution files. A caveat should 
be issued at this point. This set of programs was written in PC-Matlab version 3.5 which 
contains some newer function files not previously available. One should check the ver- 
sion of PC-Matlab employed before attempting tracking simulations. If the function files 
necessary to run these programs are not present, an error will result. The programs are 
commented throughout in the hope that they will aid the user in understanding their 
operation. 

The main driver file for the set is titled marine. m. The subroutines are titled 
brgbrg.m. rngrng.m. brgrng.m. and doppler.m. The file errellip.m is called by each of the 
subroutines as a function file. The function file reshape. m, is also necessary for the op- 
eration of marine. m. It is contained for reference. These programs should not be con- 
sidered as validated or free oflogical or arithmetic error. 

B. PROGRAM LISTING 



% JOHN A. HUCKS II 
% FILE NAME: MARINE. M 

% CONTROL FILE FOR MULTIPLE GEOMETRY EXTENDED KALMAN FILTER TARGET 
% TRACKING SIMULATION PROGRAM. 

% FILES NECESSARY FOR THE OPERATION OF THIS PROGRAM ARE: 

% BRGBRG. M 

% RNGRNG. M 

% ERGRNG. M 

% DOPPLER. M 

% ERRELLIP.M (COURTESY OF STEPHEN L. SPEHN) 

% RESHAPE. M (FUNCTION FILE) 

% THIS PROGRAM DRIVES OTHER MATLAB M (EXECUTION) FILES WHICH SIMULATE 
% TARGET TRACKING BASED ON VARIOUS OBSERVABLES, THESE BEING BEARINGS, 
% RANGES, BEARINGS AND RANGES, OR A DOPPLER SHIFTED FREQUENCY. THE 
% PROGRAMS ARE BASED ON FUSED MULTIPLE OBSERVERS THAT ARE EITHER 
% STATIONARY OR MOVING. 

% PROGRAM SETUP 



clear, ho Id off, subplot( lll),clg,clc 
titleline = 1 
titlelinel = 



f 



y 






TARGET TRACKING SIMULATOR'; 
(OPTIMAL) 



titleline2 = 1 
titleline3 = ' 



NAVAL POSTGRADUATE SCHOOL MSEE THESIS' 
CAPTAIN JOHN A. HUCKS II USMC DEC. 1989' 



% TITLE DISPLAY 



disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 



) 

) 

) 

) 

) 

) 

) 

) 

) 

’) 

') 



disp( titleline); 
disp( tit lelinel) ; 
dispf tit leline2); 
disp( t it leline3) ; 
pause 

% OBSERVER INPUT ROUTINE 



clc 
disp( * 
dispC 
disp( * 
disp( ' 



’) 

') 



disp(' ENTER THE FIRST OBSERVERS INITIAL PARAMETERS: ' ); 
disp( ' '); 



disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 



’); 






xfol = [ 



X 

VX 

y 

vy 



% observer 
% velocity 
% observer 
% velocity 



x position (km) 1 ) 
x direction (km/hr)') 
y direction (km) ' ) 
y direction (km/hr) ! ) 



dispC YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: [ x vx y vy] f f ); 
xfols = input('xfol = 1 , * s T ) ; 
eval(['xfol = 1 , xfols ,';'] ); 
xfol = reshape(xfol ,4, 1); 



clc 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 



') 

’) 

ENTER THE SECOND OBSERVERS INITIAL PARAMETERS: 









xfo2 = [ 



X 

vx 

y 

vy 



% observer x position (km)') 

% velocity x direction (km/hr)') 
% observer y direction (km) ') 

% velocity y direction (km/hr)') 



YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: [ x vx y vy] " ); 



53 



xfo2s = input('xfo2 = 
eval([ ' xfo2 = ’ ,xfo2s, '] ); 
xfo2 = reshape(xfo2 ,4, 1); 

clc 

dispC ’); 

dispC ENTER OBSERVER MOVEMENT SPEED: '); 
dispC '); 

disp( ' spd % observer speed in km/hr ' ) 

dispC '); 
dispC '); 

spds = input(’spd = ','s'); 
eval( [ ' spd = ' , spds , ' ; ' ] ); 

clc 

dispC '); 

disp( 'ENTER DESIRED NUMBER OF OBSERVATIONS: ’); 
dispC '); 

disp( ' kmax % observation number ' ) 

dispC ’); 
dispC ’); 

dispC PLEASE MAKE THIS GREATER THAN 5 SO THAT THE KALMAN GAINS') 

disp( 'CAN STABILIZE. ' ) 

kmaxs = input ('kmax = '.'s'); 

eva 1 ( [ ' kmax = ' , kmaxs ,';']); 

clc 

disp(' ’); 

disp( 'ENTER DESIRED NUMBER OF PREDICTIONS:'); 
dispC’ ’); 

dispC ' kpred % prediction number ' ) 

dispC' '); 
dispC '); 

kpreds = input( 'kpred = ','s'); 
eval( [ ' kpred = ' , kpreds , ' ; ' ] ); 

clc 

dispC '); 

dispC ENTER OBSERVATION INTERVAL:’); 

dispC '); 

dispC' dt % observation interval') 

disp(' '); 

dispC 'THE TIME INTERVAL IS IN THE FORM: ') 
dispC ’) 

dispC dt = time/60’) 

dispC '); 

disp( 'FOR A 10 MINUTE INTERVAL ENTER: dt = 10/60') 

dts = input('dt = ','s'); 
eval( [ 'dt = ' ,dts , ' ; '] ); 

% TARGET SIMULATION PARAMETERS INPUT ROUTINE 
clc 

dispC '); 
dispC '); 

dispC FOR THE PURPOSES OF SIMULATION,'); 
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ENTER THE TARGETS MOVEMENT PARAMETERS: '); 



ENTER THIS IN THE FOLLOWING FORMAT: [ x vx ax y vy ay fo] ”); 



disp( 
disp( 
disp( 

disp( ' [ x 

disp( * vx 

disp( 1 ax 

disp( | y 

disp( f vy 

disp( ’ ay 

disp( f fo 

disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 

tgts = input ( ' tgt = ' , ' s ' ); 
eval( c' tgt = ’ , tgts , ' ; ' | 
tgt = reshape(tgt, 7 , 1); 
if ( tgt( 2,1) = 0 & tgt(5 , 1) = 
clc 

disp( ’ ’ ) ; 

disp( f ’); 
disp( ’ 
disp( ’ 



% target initial x position (km) T ) 

% target initial x velocity (km/hr)’) 

% target acceleration in x (km/hrA2)’) 
% target initial y position (km)’) 

% target initial y velocity (km/hr) 1 ) 

% target acceleration in y (km/hrA2)’) 
% transmitter rest frequency (Hz) f ) 



YOU MAY 

: >i 

^**Vr****^V*************Vr^V*Vr**^V*****'5VVf , }V**'sV****Vr , >V'»V* < 5V**** 

* CAUTION: VELOCITY OF THE TARGET MUST NOT BE ZERO. * 

■jV Vr Vr Vr •>( Vc vr -sV '/V Vr -,V Vr -V Vr -V Vr Vr ■>> * Vr sV iV * ‘V Vr Vr ‘V * Vr ‘V * Vr * Vc Vr ■sV Vr * Vr * ■jWtjW? Vr * Vr * * ">V 



0) 



FOR THE PURPOSES OF SIMULATION,’); 

ENTER THE TARGETS MOVEMENT PARAMETERS: ’); 



disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( ’ 

disp( ’ 



); 

); 



X 

vx 

ax 

y 

vy 

ay 

fo 



); 

); 



]; 



target initial x position (km)’) 

target initial x velocity (km/hr)’) 

target acceleration in x (km/hrA2)’) 
target initial y position (km)’) 

target initial y velocity (km/hr)’) 

target acceleration in y (km/hrA2)’) 
transmitter rest frequency (Hz)’) 



end 



disp( ' YOU MAY ENTER THIS 
[x vx ax y vy ay fo] ’ ’); 
disp( ’ ’); 

disp( 1 ’); 

disp( ’ 
disp( ' 
disp( ’ 

tgts = input( ’ tgt = ’ , ’ s 1 ); 
eval([ ’tgt = * j tgts y ';*]); 
tgt = re shape ( tgt ,7,1); 



IN THE FOLLOWING FORMAT: 



j# ju j# ju j#*,*# *.* « j* j# j* y* y# y* y# y* jLy«y*y* y« y# y» y* y^> 

* CAUTION: VELOCITY OF THE TARGET MUST NOT BE ZERO. * 

j#.** jj* Vr Vr V* VrVc Vr Vr Vr Vr Vr Vr Vr^V'/WrVrVr Vr^V 

input( ' tgt = 



global xfol xfo2 spd kmax kpred dt tgt; 

% OBSERVABLE MENU CHOICE ROUTINE 

choicel = -10; 
while choicel = 0 
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c lc 

dispC ’); 
disp( ’ ’); 

dispC' '); 
disp( tit le line); 
disp( titlelinel); 
dispC’ ’); 

); 



disp( ’ 
dispC ’ 
dispC 1 
dispC ! 
dispC 1 
dispC * 
dispC 1 
disp( r 
dispC T 
dispC ! 
dispC * 
dispC r 
dispC f 
dispC T 
dispC ’ 
dispC ’ 
disp( ’ 
dispC’ 
dispC ’ 



); 



); 

); 



ENTER THE METHOD OF OBSERVATION USED FOR THIS RUN: '); 

1. Bearing/Bearing’ ); 

2. Bearing/Range’ ); 

3. Range/Rangej; 

4. Shifted Doppler Frequency 1 ); 

5. Display the last graph’); 

6. Change the number of observations’); 

7. Change the number of predictions’); 

8. Change the observation interval’); 

9. Change position of first observer’); 

10. Change position of second observer’); 

11. Change observer movement speed’); 

12. Change target simulation parameters’); 



0. Quit ’ ); 

ENTER YOUR CHOICE: ’ ); 












= 6 



choicel = input ( 1 
if choicel == 1 
brgbrg; 

elseif choicel 
brgrng; 

elseif choicel 
rngrng; 

elseif choicel 
doppler; 
elseif choicel 
shg 
pause 

elseif choicel 
clc 

dispC T T ); 

dispC f ENTER DESIRED NUMBER OF OBSERVATIONS: ’); 
dispC; ’); 

dispC f kmax % observation number ’ ) 

disp(’ f ); 
dispC T ! ); 

dispC ’PLEASE MAKE THIS GREATER THAN 5 SO THAT THE KALMAN GAINS’) 
dispC * CAN STABILIZE. ’ ) 
kmaxs = input (’kmax = ’,’s’); 
eval( [ 1 kmax = ’ , kmaxs , ’ ; * ] ); 
elseif choicel == 7 
clc 

dispC 1 ! ); 

dispC ’ENTER DESIRED NUMBER OF 
dispC | ! ); 

dispC ! kpred 



PREDICTIONS: ’); 

% prediction number * ) 
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disp( ' '); 
disp( '); 

kpreds = input('kpred = ','s'); 
eval( [ ' kpred = ' ,kpreds , ' ; ' ] ); 
elseif choicel = 8 
clc 

dispC’ '); 

dispC ENTER OBSERVATION INTERVAL:'); 
dispC ' ') 

disp( ' dt 

disp( ' '); 

dispC THE TIME INTERVAL IS IN THE FORM: ') 
dispC ’) 

disp( ' dt = time/60') 

dispC '); 

dispC FOR A 10 MINUTE INTERVAL ENTER: dt = 10/60') 

dispC '); 

dts = input('dt = ','s'); 
eval([ 'dt = ’ ,dts,’C] ); 
elseif choicel = 9 
clc 

dispC '); 
disp(' '); 
dispC '); 
dispC '); 

disp( ' ENTER THE FIRST OBSERVERS INITIAL PARAMETERS:'); 
dispC '); 



% observation interval') 



disp( ' 
disp( ' 
disp( ' 
dispC ' 
dispC ' 
disp( ' 



); 



’); 



xfol = [ 



X 

vx 

y 

vy ] ; 



% observer x position Ckm) ' ) 

% velocity x direction Ckm/hr) ') 

% observer y direction Ckm)') 

% velocity y direction Ckm/hr)') 



dispC YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: [ x vx y vy] 
xfols = input('xfol = ','s'); 
evalC['xfol = ', xfols ,';']) ; 
xfol = reshapeCxfol ,4, 1); 
elseif choicel = 10 
clc 

disp(' '); 

dispC '); 
dispC' '); 
dispC '); 

dispC 'ENTER THE SECOND OBSERVERS INITIAL PARAMETERS:'); 
dispC' '); 



); 



dispC ' 

dispC' 
dispC ' 
dispC ' 
dispC ' 
dispC ' 



'); 



’); 



xfo2 = [ 



X 

vx 

y 

vy ] ; 



% observer 
% velocity 
% observer 
% velocity 



x position (km) 1 ) 
x direction (km/hr)') 
y direction (km)') 
y direction (km/hr)') 



dispC YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: [ x vx y vy] tf ); 



xfo2s = input(*xfo2 = 



_ t 






evalC[ 'xfo2 = ' ,xf°2s,'; '] ); 
xfo2 = reshapeCxfo2 ,4 , 1); 
elseif choicel = 11 
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clc 

dispC '); 

disp( ' ENTER OBSERVER MOVEMENT SPEED: ' ); 
disp( ' '); 

disp( ' spd % observer speed in km/hr 1 ) 

disp(' ’); 
dispC ’); 

spds = input(’spd = 1 , 1 s ' ) ; 
eval([ 1 spd = ’ ,spds,'; f ] ); 
elseif choicel — 12 
clc 

disp( ' '); 

dispC ’); 
disp( ' 
disp( ' 

dispC '); 
dispC '); 

disp( 1 
disp( ' 
disp( 1 
disp( 1 
disp( 1 
disp( ' 
disp( ' 
dispf ' 1 ) ; 

disp( '); 

dispC YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: 

[ x vx ax y vy ay f o] 1 ' ) 



FOR THE PURPOSES OF SIMULATION, 1 ); 

ENTER THE TARGETS MOVEMENT PARAMETERS: f ); 



[ x % target initial x position (km)') 

vx % target initial x velocity (km/hr) 1 ) 

ax % target acceleration in x (km/hrA2)') 

y % target initial y position (km)') 

vy ?o target initial y velocity (km/hr)') 

ay % target acceleration in y (km/hrA2)’) 

fo ] ; /o transmitter rest frequency (Hz) 1 ) 



disp( 1 '); 

disp( ' '); 

d i s p ( ' *****?•?***?• 
disp( ’ * CAUTION: 
d i s p ( ' ********** 
tgts - input('tgt = 
eval([ ' tgt = ,tgts , 
tgt = reshape( tgt , 7 , 
if ( tgt( 2 , 1) == 0 
clc 

disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 
disp( ' 






); 

); 



J. J. J. A A AA A A A A. A A A A A A JL. A A AAAJ^ I j 

VELOCITY OF THE TARGET MUST NOT BE ZERO. * | ) 

' ,'s’); 

i); 

& tgt(5 , 1) = 0) 



FOR THE PURPOSES OF SIMULATION,'); 

ENTER THE TARGETS MOVEMENT PARAMETERS: ' ); 






X 

vx 

ax 

y 

vy 
ay 
fo ] 



% target initial x position (km)') 

% target initial x velocity (km/hr)') 

% target acceleration in x (km/hrA2)') 
% target initial y position (km)') 

% target initial y velocity (km/hr)') 

% target acceleration in y (km/hrA2)') 
% transmitter rest frequency (Hz)') 



disp(' YOU MAY 
[ x vx ax y vy 



ENTER THIS IN THE FOLLOWING FORMAT: 
ay fo] ' ' ); 
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dispC ’); 
disp(’ '); 

disp( * ***************************************************** ) 

disp( ' * CAUTION: VELOCITY OF THE TARGET MUST NOT BE ZERO. *’) 
d i sp ( ' ************?>**************************************** 1 ) 
tgts = input('tgt = ’ , ' s ' ); 
eval( [ ' tgt = ' , tgts , ' ; '] ); 
tgt = reshape( tgt , 7 , 1); 

end 

end; 

end; 



% JOHN A. HUCKS II 
% FILE NAME : BRGBRG. M 

% EXTENDED KALMAN FILTER PROGRAM USING BEARING/ BEARING MEASUREMENTS 

?; THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 
% FILTER, WITH INPUTS FROM A FUSED SENSOR ARRAY AS IT TRACKS A TARGET 
% ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH PROJECTION). 



clg 

clear eex eey xto 
! erase brgbrg. met 
diary bearing 

% STATE EQUATIONS 

% OBSERVATION TIME INTERVAL (input from marine. m) 

% STATE TRANSITION MATRIX 
phi = [ 1 dt 0 0 

0 10 0 
0 0 1 dt 



0 0 0 


i ]; 


SYSTEM NOISE 


COEFFICIENT MATRIX 


= [ dt*dt/2 


0 


dt 


0 


0 


dt*dt/2 


0 


dt ]; 



% STATE VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 
% NUMBER OF OBSERVATIONS (input from marine. m) 



%, ESTIMATED TARGET STATE 

xte = zeros(4,kmax); % initial est. target state 

% OBSERVER (SENSOR) INITIAL POSITIONS AND VELOCITIES 
% (input from marine. m) 

% OBSERVER SPEED (input from marine. m) 
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% INITIAL TIME SETTING 
Time = zeros( 1 ,kmax); 



% ACTUAL TRACK POSITIONS (tgt vector input from marine, m) 
x = zeros( 1 ,kmax); 
y = zeros( 1 ,kmax); 

T = 0; 
for k = 1 

x(k) = tgt( 1,1); 
y(k) = tgt(4,l); 

end 

for k = 2: kmax 

T = T+dt; 

x(k) = (0. 5*tgt(3, 1 )*(Ta 2))+( tgt( 2, 1). *T)+( tgt( 1, 1) ); 
y(k) = ( 0. 5*tgt( 6 , 1 )*(Ta 2) )+( tgt(5 , 1). *T)+( tgt(4, 1)); 

end 

zpos = [ x 

y ]; 



INPUT OBSERVATION VALUES ( BEARING/BEARING) 
INITIAL OBSERVED TARGET STATE 



xto( : , 1) = [ tgt( 1 , 1) 
tgt(2,l) 
tgt(4, 1) 
tgt(5, 1) ] ; 



% (km in 
% (km/hr 
% (km in 
% (km/hr 



x direction) 
in x direction) 
y direction) 
in y direction) 



% SIMULATION COUNTER LOOP 
xfoa = zeros(4,kmax); 
xfoa( : , 1) = xfol( : ,1); 
xfol = xfoa; 
xfob = zeros(4,kmax); 
xfob(: ,1) = xfo2(: ,1); 
xfo2 = xfob; 
k = 0; 

for kl=l: 10*kmax 

k = k + 1; % actual loop counter 

if (k == kmax) % check for termination 

break 
end 



o/ 

/o 



UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 
way = pi/2; 
else 

way = - (3*pi/4); 
end 



if spd = 0 
xfol(l,k+l) 
xfol(3,k+l) 
xfo2(l,k+l) 
xfo2( 3 ,k+l) 
else 

xfol( 1 ,k+l ) 
xfol(2,k+l) 
xfol(3,k+l) 
xfol(4,k+l) 



xfol( 1 ,k); 
xfol( 3,k); 
xfo2( 1 ,k); 
xfo2( 3 ,k) ; 

xfol(l,k) + spd*sin(way)*dt; 
spd- v sin(way); 

xfol(3,k) + spd*cos(way) ,v dt; 
spd*cos(way); 
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xfo2(l,k+l) = xfo2(l,k) + spd*sin(way)*dt; 
xfo2(2,k+l) = spd*sin(way); 
xfo2(3,k+l) = xfo2(3,k) + spd*cos(way)*dt; 
xfo2(4,k+l) = spd*cos( way); 
end 
end 

deltlx = zpos(l,:) - xfol(l,:); 
deltly = zpos(2,: ) - xfol(3,: ); 
delt2x = zpos(l,:) - xfo2(l,:); 
delt2y = zpos(2,:) - xfo2(3,:); 

brgl = atan2(deltlx, deltly); % bearing thtal 

brg2 = atan2(delt2x,delt2y); % bearing thta2 

zobs = [ brgl; brg2] ; 
xtol = zeros(4,kmax); 
xtol(l,:) = zpos(l,:); 
xtol(3,:) = zpos(2,:); 
xtol(: ,1) = xto(: ,1); 
xto = xtol; 

% STATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 
varv = 0.0001; % variance of linear acceleration 

varth = 0. 01096; % variance of angular acceleration 

R = [ 0. 000005 0 

0 0. 000005 ] ; % measurement noise 

% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ' normal ' ) 

zobs = zobs + sqrt(R( 1 , 1) )*rand(zobs); 

% INITIAL KALMAN MATRICES AND OBSERVATION 
I = eye(4); 

% INITIAL KALMAN P MATRIX 
P0 = [ 800 0 0 0 

0 800 0 0 

0 0 800 0 

0 0 0 800 ]; 

% REINITIALIZING P MATRIX 
P = P0; 

% KALMAN ESTIMATE 
xte( : , 1) = [ xto( 1,1) 
tgt( 2 , 1) 
xto( 3,1) 
tgt(5 , 1) ]; 

tpred = xte( : , 1); 

% SIMULATION OF KALMAN FILTER OPERATION 
% NUMBER OF BAD OBSERVATIONS 

nbad =0; % initial setting bad obs. cntr. 

% SIMULATION COUNTER LOOP 
k = 0; 

for kl=l: 10*kmax 



% reset value of Error Covariance 



% (km - x direction) 
% (km/hr) 

% (km - y direction) 
% (km/hr) 
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% actual loop counter 
% check for termination 



k = k + 1; 
if (k == kmax) 
break 

end 



UPDATE FORWARD OBSERVER'S/ FAC’S POSITION (IF THEY ARE MOVING) 
if k <= 3 
way = pi/2; 
else 



way = -( 3*pi/ 4) ; 

end 



if spd = 0 
xfol( 1 ,k+l) 
xfol( 3 ,k+l) 
xfo2( 1 ,k+l) 
xfo2(3,k+l) 
else 



xfol( 1 ,k) 
xfol(3,k) 
xfo2(l,k) 
xfo2(3,k) 



xfol( 1 ,k+l ) 
xfol(2,k+l) 
xfol( 3 ,k+l) 
xfol(4,k+l) 
xfo2( 1 ,k+l) 
xfo2( 2 ,k+l) 
xfo2( 3 , k+1) 
xfo2(4 ,k+l) 

end 



xfol(l,k) + spd*sin(way)*dt; 
spd*sin(way); 

xfol(3,k) + spd*cos(way)*dt; 
spd* cos (way); 

xfo2(l,k) + spd*sin(way) ,v dt; 
spd*sin(way); 

xfo2(3,k) + spd*cos(way)*dt; 
spd-'-cos(way); 



°,o PROJECT AHEAD KALMAN STATE ESTIMATE 
xte(: ,k+l) = phi*xte(: ,k); 

% UPDATE Q 

vt = sqrt( (xte( 2 ,k+l) ) a 2 + (xte( 4 ,k+l) ) a 2); 
qll = varv * (xte(2,k+l) / vt)*2 + varth * (xte( 4 ,k+l ) ) a 2; 
q22 = varv * (xte(4,k+l) / vt)A2 + varth * (xte( 2 ,k+l) ) a 2; 
q 12 = xte(2,k+l) * xte(4,k+l) * ((varv/vt)A2 - varth ); 
q21 = q 1 2 ; 

Q1 = [ qll ql2 

q21 q22 1; 

Q = del*Ql*(del ' ); 



% PROJECT ERROR COVARIANCE 
P = phi*P*(phi ' ) + Q; 

% UPDATE H 

deltlx = xte(l,k+l) - xfol(l,k+l); 
deltly = xte(3,k+l) - xfol(3,k+l); 
delt2x = xte(l,k+l) - xfo2(l,k+l); 
delt2y = xte(3,k+l) - xfo2(3,k+l); 
rl2 = (deltlx) 2 + (deltly) 2; 
r22 = ( delt2x) 2 + (delt2y) 2; 
rl = sqrt(rl2); 
r2 = sqrt(r22); 

H = [ deltly/rl2 0 -deltlx/rl2 0 

delt2y/r22 0 -delt2x/r22 0 ]; 

% GET NEW MEASURED ESTIMATE 
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zhatl = atan2(deltlx,deltly); 
zhat2 = atan2(delt2x,delt2y); 
zhat = [ zhatl 



% bearing estimate thtal 
% bearing estimate thta2 



zhat 2 ] ; 



% COMPUTATION OF KALMAN GAIN 
vresid = (H*P*(H') + R); 

G = P*(H' ) / vresid; 



% variance of the residual 
% Kalman gain 



% COMPUTATION OF UPDATED ERROR COVARIANCE 
P = (I - G*H)*P; 

% COMPUTATION OF RESIDUAL 
resid = zobs(: ,k+l) - zhat; 

% IF RESIDUAL IS TOO BIG, THEN RESET P AND DO CALCULATIONS AGAIN, 
if ( abs( resid( 1) ) > 1. 0*sqrt( abs( vresid( 1 , 1) ) ) ... 

] abs(resid(2)) > 1. 0*sqrt( abs( vresid( 2 , 2) ) ) ... 

& nbad — 0) 
nbad = 1; 
k = k-1; 

P = PO; % reset P 

else 

% COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+l) = xte(: ,k+l) + G Vf resid; 

% ERROR ELLIPSOID PLOTTING ROUTINE 
M = [ xte(l,k+l) xte(3,k+l) ]’; 

K = [ P(l,l) P(l, 3) 

P( 3 , 1) P( 3 , 3) ]; 

[xp,yp] = errellip(M,K, . 60,50,0); 
eex( : ,k) = xp; 
eey( : ,k) = yp; 

% PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
% (number of predictions desired input from marine, m) 
tpred(: ,1) = xte(: ,k+l); 
acc = zeros( 2 , 1 ); 

for 1 = lrkpred % kpred is # of predictions 

if k >= 3 

accx = (xte(2,k+l)-xte(2,k))/dt; 
accy = (xte(4,k+l) -xte(4,k) )/dt; 
acc = [ accx; accy] ; 
if accx >= 5 



break 

acc = zeros(2,l); 
elseif accy >= 5 
break 

acc = zeros(2, 1); 

end 



nbad = 0; 

end 

% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 



end 



tpred(: ,1+1) = phi*tpred(: , l)+del*acc; 



end 
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plot(xfol( 1 , 1: k+1) ,xfol( 3 , 1: k+1) , ' xw' ,xto( 1,1: k+1) ,xto( 3 , 1: k+1) 

,’.b' .... 

xte( 1,1: k+1) ,xte( 3,1: k+1) , '+w' ,xfo2(l,l: k+1) ,xfo2( 3 , 1: k+1) ' xw' . 
zpos( 1 , 1: k+1) , zpos( 2 , 1: k+1) , ' ow' ,tpred( 1 ,: ) ,tpred( 3,: ) , '*g . 

eex , eey , ' -r ' ) , gr id 

title( ’ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS USING BRG/BRG ’) 

x’labelC 'DISTANCE X’ ) ,ylabel( ' DISTANCE Y') 
end ♦ 

meta brgbrg 



% JOHN A. HUCKS II 
% FILE TITLE: RNGRNG. M 

% EXTENDED KALMAN FILTER PROGRAM USING RANGE/RANGE MEASUREMENTS 

% 

% THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 
% FILTER, WITH INPUTS FROM A FUSED SENSOR ARRAY AS IT TRACKS A TARGET 
% ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH PROJECTION). 



clg 

clear eex eey xto 
! erase rngrng. met 
diary rngrng 

% STATE EQUATIONS 

% OBSERVATION TIME INTERVAL (input from marine. m) 

% STATE TRANSITION MATRIX 
phi = [ 1 dt 0 0 

0 10 0 
0 0 1 dt 

0 0 0 1 ] ; 

% SYSTEM NOISE COEFFICIENT MATRIX 
del = [ dt‘ ; 'dt/2 0 

dt 0 

0 dt'"dt/2 

0 dt ] ; 

% STATE VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 
% NUMBER OF OBSERVATIONS (input from marine, m) 

% ESTIMATED TARGET STATE 

xte = zeros(4,kmax); % initial est. target state 

% OBSERVER (SENSOR) INITIAL POSITIONS AND VELOCITIES 
% (input from marine, m) 

% OBSERVER SPEED (input from marine, m) 
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% INITIAL TIME SETTING 
Time = zeros( 1 ,kmax) ; 

% ACTUAL TRACK POSITIONS (tgt vector input from marine. m) 
x = zeros( 1 ,kmax); 
y = zeros( 1 ,kmax); 

T = 0; 
for k = 1 

x(k) = tgt( 1,1); 
y(k) = tgt(4,l); 

end 

for k = 2: kmax 

T = T+dt; 

x(k) = ( 0. 5*tgt( 3 , 1 )*(Ta 2) )+( tgt( 2,1). *T)+( tgt( 1,1)); 
y(k) = ( 0. 5*tgt( 6 , 1 )*(Ta 2) )+( tgt( 5,1). *T)+( tgt(4, 1) ); 

end 

zpos = [ x 

y ]; 

% INPUT OBSERVATION VALUES (RANGE/RANGE) 

% OBSERVED TARGET STATE 
xfoa = zeros(4 ,kmax); 
xfoa(: ,1) = xfol(: ,1); 
xfol = xfoa; 
xfob = zeros(4,kmax); 
xfob(: ,1) = xfo2(: ,1); 
xfo2 = xfob; 
xto( : ,1) = [ tgt(l,l) 
tgt (2 , 1) 
tgt(4 , 1) 
tgt( 5,1) ] ; 

% SIMULATION COUNTER LOOP 
k = 0; 

for kl=l: 10*kmax 
k = k + 1; 
if (k = kmax) 
break 

end 



% (km in x direction) 

% (km/hr in x direction) 
% (km in y direction) 

% (km/hr in y direction) 



% actual loop counter 
% check for termination 



0 / 

/o 



UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 



way = pi/2; 
else 



way = -( 3*pi/ 4); 

end 



xfol( 1 ,k+l) 
xfol( 3 ,k+l) 
xfo2(l,k+l) 
xfo2( 3 ,k+l) 
else 

xfol( 1 ,k+l) 
xfol( 2 ,k+l) 
xfol(3,k+l) 
xfol(4,k+l) 
xfo2( 1 ,k+l) 



xfol( 1 ,k); 
xf ol( 3 ,k) ; 
xfo2( 1 ,k); 
xfo2(3,k); 

xfol(l,k) + spd*sin(way)*10*dt; 
spd*sin(way); 

xfol(3,k) + spd*cos(way)*10*dt; 
spd ,v cos(way); 

xfo2(l,k) + spd*sin(way)*10*dt; 
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xfo2(2,k+l) = spd*sin(way); 

xfo2(3,k+l) = xfo2(3,k) + spd*cos(way)*10*dt; 
xfo2(4,k+l) = spd ,v cos(way); 

end 

end 

rl = sqrt( ( zpos( 1 ,: ) -xfol( 1 , : ) ). a2 + ( zpos( 2 , : ) -xfol( 3 , : ) ). a2); 

r2 = sqrt((zpos( 1,: )-xfo2( 1, : )). a2 + ( zpos( 2 , : ) -xfo2( 3, : ) ) . a2); 

zobs=[ rl; r2] ; 

xtol = zeros(4,kmax); 

xtol(l,: ) = zpos(l,: ); 

xtol(3,: ) = zpos(2,:); 

xtol(: ,1) = xto(: ,1); 

xto = xtol; 

% STATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 

varv =5.0; % variance of linear acceleration 

varth = 1. 0; % variance of angular acceleration 

R = [ 0. 005 0 

0 0. 005 ] ; % measurement noise 

% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ' normal ' ) ; 

zobs = zobs + sqrt(R( 1, 1) )*rand(zobs); 



% INITIAL KALMAN MATRICES AND OBSERVATION 



I = ey e ( 4 ) ; 

% INITIAL KALMAN P MATRIX 
P0 = [ 250 0 0 0 

0 250 0 0 

0 0 250 0 

0 0 0 250]; 

% REINITIALIZING P MATRIX 
P = P0; 

% KALMAN ESTIMATE 
xte( : , 1) = [ xto( 1,1) 
tgt(2,l) 
xto( 3,1) 
tgt(5,l) ]; 



% reset value of Error Covariance 



% (km - x direction) 
% (km/hr) 

% (km - y direction) 
% (km/ hr) 



tpred = xte( : , 1); 



% SIMULATION OF KALMAN FILTER OPERATION 
% NUMBER OF BAD OBSERVATIONS 

nbad =0; % initial setting bad obs. cntr. 

% SIMULATION COUNTER LOOP 
k = 0; 

for kl=l: 10*kmax 

k = k + 1; % actual loop counter 

if (k = kmax) % check for termination 

break 

end 



% UPDATE FORWARD OBSERVER’S/ FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 
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way = pi/2; 
else 



way = -( 3*pi/4) ; 

end 



if spd = 0 
xfol( 1 ,k+l) 
xfol( 3 ,k+l) 
xfo2( 1 ,k+l) 
xfo2(3,k+l) 
else 

xfol( 1 ,k+l) 
xfol( 2 ,k+l) 
xfol( 3 ,k+l) 
xfol(4,k+l) 
xfo2( 1 ,k+l) 
xfo2( 2 ,k+l) 
xfo2( 3 ,k+l) 
xfo2(4,k+l) 



xfol( 1 ,k) ; 
xfol(3,k); 
xfo2( 1 ,k); 
xf o2( 3 ,k) ; 

xfol(l,k) + spd*sin(way)*10*dt; 
spd*sin(way); 

xfol(3,k) + spd*cos(way)*10*dt; 
spd*cos(way); 

xfo2(l,k) + spd*sin(way)*10*dt; 
spd*sin(way); 

xfo2(3,k) + spd*cos(way)*10*dt; 
spd*cos( way); 



% PROJECT AHEAD KALMAN STATE ESTIMATE 



xte(: ,k+l) = phi*xte(: ,k); 

% UPDATE Q 

vt = sqrt( (xte( 2 ,k+l ) ) 2 + (xte(4,k+l)) 2); 
qll = varv * (xte(2,k+l) / vt) 2 + varth * (xte(4,k+l)) 2; 

q22 = varv * (xte(4,k+l) / vt) 2 + varth * (xte(2,k+l)) 2; 

ql2 = xte(2,k+l) * xte(4,k+l) * ((varv/vt) 2 - varth ); 
q21 = ql2; 

Q1 = [ qll ql2 

a21 q22 1; 

Q = del*Ql*(der); 

% PROJECT ERROR COVARIANCE 
P = phi*P*(phi') + Q; 

% UPDATE H 

deltlx = xte(l,k+l) - xfol(l,k+l); 
deltly = xte(3,k+l) - xfol(3,k+l); 
delt2x = xte(l,k+l) - xfo2(l,k+l); 
delt2v = xte(3,k+l) - xfo2(3,k+l); 
rl2 = (deltlx) 2 + (deltly) 2; 

r22 = (delt2x) 2 + (delt2y) 2; 

rl = sqrt(rl2); 
r2 = sqrt(r22); 

H = [ deltlx/rl 0 deltly/rl 

delt2x/r2 0 delt2y/r2 

% GET NEW MEASURED ESTIMATE 

zhat = [ rl 

r2 ]; 

% COMPUTATION OF KALMAN GAIN 
vresid = (H*P*(H') + R)j 
G = P*(H' ) / vresid; 



0 

0 ]; 



% variance of the residual 
% Kalman gain 
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% COMPUTATION OF UPDATED ERROR COVARIANCE 
P = (I - G*H)*P; 

% COMPUTATION OF RESIDUAL 
resid = zobs(: ,k+l) - zhat; 

% IF RESIDUAL IS TOO BIG, THEN RESET P AND DO CALCULATIONS AGAIN, 
if (abs(resid( 1)) > 1. 0 ,v sqrt( abs( vresid( 1 , 1) ) ) ... 

] abs(resid(2)) > 1. 0*sqrt( abs( vresid( 2 , 2) ) ) ... 

& nbad = 0) 
nbad = 1; 
k = k-1; 

P = PO; % reset P 

else 

% COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+l) = xte(: ,k+l) + G*resid; 

% ERROR ELLIPSOID PLOTTING ROUTINE 
M = (xte(l,k+l) xte(3,k+l)] *; 

K = [ P(l,l) P( 1 , 3) 

pf 3 1 ) Pf 3 3^ 1 • 

[xp,yp] = errellip(M,K,. 60,25,0); 
eex( : ,k) = xp; 
eey( : ,k) = yp; 

% PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
% (number of predictions desired input from marine, m) 
tpred(: ,1) = xte(: ,k+l); 
acc = zeros( 2 ,1); 

for 1 = l:kpred % kpred is # of predictions 

if k >= 5 

accx = ( xte( 2 ,k+l ) -xte( 2 ,k) ) /dt; 
accy = (xte(4,k+l)-xte(4,k))/dt; 
acc = [ accx; accy] ; 
if accx >= 5 
break 

acc = [ 0; 0] ; 
elseif accy >= 5 
break 

acc = [ 0; 0] ; 

end 

end 

tpred(: ,1+1) = phi*tpred(: ,l)+del*acc; 

end 

nbad = 0; 

end 

% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
plot(xfol(l,l:k+l),xfol(3,l:k+l),’xw , ,xte(l,l:k+l), 
xte(3, 1: k+1) , ' +w ’ ,. . . 

xfo2( 1 , 1: k+1) ,xfo2( 3 , 1: k+1) , ’ xw’ ,zpos( 1,1: k+1) ,zpos( 2,1: k+1) , * ow* , . . . 
xto(l,l:k+l),xto(3,l:k+l), , .b , ,tpred(l,: ), tpred( 3 , : ) , 1 *g’ , . . . 
eex,eey , 1 -r ’ ) ,grid 

t it le( ’ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS 
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USING RNG/RNG OBS' ),. . . 

xlabel( ’DISTANCE X' ) ,ylabel( ' DISTANCE Y') 

end 

meta rngrng 
diary off 



% JOHN A. HUCKS II 
% FILE NAME : BRGRNG. M 

% EXTENDED KALMAN FILTER USING BEARING/RANGE MEASUREMENTS 

% THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 
% FILTER WITH INPUTS FROM A FUSED SENSOR ARRAY, AS IT TRACKS A 
% POTENTIAL TARGET ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH 
% PROJECTION). 



clg 

clear eex eey xto 
! erase brgrng. met 
diary brgrng 

% STATE EQUATIONS 

% OBSERVATION TIME INTERVAL (input from marine, m) 

% STATE TRANS I ST I ON MATRIX 
phi = [ 1 dt 0 0 

0 10 0 
0 0 1 dt 

0 0 0 1 ] ; 

% SYSTEM NOISE COEFFICIENT MATRIX 
del = [ dt*dt/2 0 

dt 0 

0 dt*dt/2 

0 dt ] ; 

% STATE VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 
% NUMBER OF OBSERVATIONS (input from marine, m) 



% ESTIMATED TARGET STATE 

xte = zeros(4,kmax); % estimated target state 

% OBSERVER (SENSOR) INITIAL POSITIIONS AND VELOCITIES (input from marine. m) 

% OBSERVER SPEED (input from marine, m) 

% INITIAL TIME SETUP 
Time = zeros( 1 ,kmax); 

% ACTUAL TRACK POSITIONS (tgt vector input from marine, m) 
x = zeros( 1 ,kmax); 
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y = zeros( 1 ,kmax); 

T = 0; 
for k = 1 

x(k) = tgt( 1,1); 
y(k) = tgt(4, 1); 

end 

for k = 2: kmax 

T = T+dt; 

x(k) = ( 0. 5*tgt( 3 , 1 )*(Ta 2) )+(tgt( 2, l)*T)+(tgt( 1,1)); 
y(k) = ( 0. 5*tgt( 6 , 1 )*(Ta 2) )+( tgt(5 , l)*T)+(tgt(4, 1) ); 

end 

zpos = [ x 

y ]; 

% INPUT OBSERVATION VALUES (BEARING/RANGE): 

xfoa = zeros(4,kmax); 

xfoa( : , 1) = xfol( : , 1); 

xfol = xfoa; 

xfob = zeros(4,kmax); 

xfob(: ,1) = xfo2(: ,1); 

xfo2 = xfob; 

xfor = zeros(4,kmax); 

zobr = zeros( 1: kmax); 

rngl = zeros( 1: kmax) ; 

rng2 = zeros( 1: kmax); 

deltlx = zeros( 1: kmax); 

delt2x = zeros( 1: kmax); 

deltly = zeros( 1: kmax) ; 

delt2y = zeros( 1: kmax) ; 

thtal = zeros( 1: kmax) ; 

thta2 = zeros( 1: kmax); 

for k = 1 : kmax 

rngl(k) = sqrt( ( (zpos( 1 ,k)-xfol( 1 ,k)). a2) + ... 

((zpos(2,k)-xfol(3,k)). a 2)); 
rng2(k) = sqrt ( ( ( zpos( 1 , k) -xf o2( 1 , k) ) . a2) + ... 

( ( zpos( 2,k)-xfo2(3,k)). a 2)); 
deltlx(k) = zpos( 1 ,k) -xfol( 1 ,k); 
deltly(k) = zpos( 2 ,k) -xfol( 3 ,k); 
delt2x(k) = zpos( 1 ,k)-xfo2( 1 ,k); 
delt2y(k) = zpos( 2 ,k) -xfo2( 3 ,k); 
thtal(k) = atan2( delt lx(k) ,deltly(k)); 
thta2(k) = atan2(delt2x(k) 5 delt2y(k)); 

end 

zobr = [ thtal; rngl; thta2; rng2 ]; 

% MEASUREMENT REFERENCE CONVERSION ROUTINE: 
for cnt = 1 

xtl = tgt ( 1,1); 

xt2 = tgt( 1,1); 

ytl = tgt(4,l); 

yt2 = tgt ( 4 , 1 ) ; 

xtr = (xtl+xt2)/2; 

ytr = (ytl+yt2)/2; 

delxr = xtr( 1 ,cnt); 

delyr = ytr(l,cnt); 

thtar(l,cnt) = atan2( delxr , delyr) ; 
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rngr(l,cnt) = sqrt((delxr 2)+(delyrA2) ); 

end 

for cnt = 2: kmax 

xtl(l,cnt) = zobr(2 , cnt)*sin(zobr( l,cnt) ); 
ytl(l,cnt) = zobr( 2,cnt)*cos(zobr( 1 ,cnt)); 
xt2(l,cnt) = zobr(4,cnt)*sin(zobr(3,cnt)); 
yt2(l,cnt) = zobr(4,cnt)*cos(zobr(3,cnt)); 
xtr(l,cnt) = (xtl( l,cnt)+xt2( l,cnt))/2; 
ytr( 1 , cnt) = (yt 1( 1 , cnt )+yt2( 1 , cnt) )/2; 
delxr = xtr(l,cnt); 
delyr = ytr(l,cnt); 
thtar(l,cnt) = atan2(delxr, delyr); 
rngr(l,cnt) = sqrt((delxr 2)+(delyr a2) ); 

end 



% CONVERTED OBSERVATION MATRIX: (CONVERSION OBSERVER TAKEN AS THE ORIGIN) 
% OBSERVED TARGET STATE 



xto(: ,1) = [ tgt(l,l) 
tgt(2, 1) 
tgt(4,l) 
tgt(5, 1) ] ; 
zobs = [ thtar 
rngr ]; 

xtol = zeros(4,kmax); 
xtol(l,:) = zpos(l,:); 
xtol(3,:) = zpos(2,:); 
xtol( : , 1) = xto(: ,1); 
xto = xtol; 



% (km in x direction) 

% (km/hr in x direction) 
% (km in y direction) 

% (km/hr in y direction) 



% STATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 



varv = 5. 0; 
varth = 1. 0; 

R = [ 0. 0005 


0 


% variance of linear acceleration 
% variance of angular acceleration 


0 


0. 005 1 ; 


% measurement noise 



% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ' normal ' ) 

noise = sqrt(R( 1 , 1) )*rand( zobs) ; 

% INITIAL KALMAN MATRICES AND OBSERVATIONS 
I = eye(4); 

% INITIAL’ KALMAN P MATRIX 

PO = [ 700 000 % reset value of error covariance 

0 700 0 0 

0 0 700 0 

0 0 0 700 ]; 

% REINITIALIZING P MATRIX 
P = P0; 

% INITIAL KALMAN ESTIMATE 
xte(: ,1) = [ xto( 1 > 1 ) 
tgt(2, 1) 

xto(3, 1) 
tgt( 5 , 1) ]; 
tpred = xte( : , 1); 
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SIMULATION OF FUSED SENSOR OPERATION USING EXTENDED KALMAN FILTER 
NUMBER OF BAD OBSERVATIONS 
nbad =0; % initial setting bad obs. cntr. 

k = 0; 

for kl=l:50*kmax 

k = k + 1; % actual loop counter 

if (k = kmax) % check for termination 

break 

end 



% UPDATE OBSERVER'S/FAC’S POSITION (IF THEY ARE MOVING) 
if k <= 3 
way = pi/2; 
else 

way = -pi/4; 

end 



xfol( 1 ,k+l) 
xfol(3,k+l) 
xfo2( 1 ,k+l) 
xfo2( 3,k+l) 

else 

xfol( 1 ,k+l) 
xfol( 2 ,k+l) 
xfol( 3 ,k+l) 
xfol(4,k+l) 
xfo2( 1 ,k+l) 
xfo2( 2 ,k+l) 
xfo2(3,k+l) 
xfo2(4,k+l) 

end 



xfol( 1 ,k); 
xfol( 3,k); 
xfo2( 1 ,k); 
xfo2( 3 , k) ; 

xfol(l,k) + spd*sin(way)*dt; 
spd ,v sin(way); 

xfol(3,k) + spd*cos(way)*dt; 
spd*cos( way) ; 

xfo2(l,k) + spd*sin(way)*dt; 
spd*sin(way); 

xfo2(3,k) + spd*cos(way)*dt; 
spd-' f cos(way); 



% PROJECT AHEAD KALMAN STATE ESTIMATE 
xte(: ,k+l) = phi ,v xte(: ,k); 

% UPDATE Q 

vt = sqrt( (xte( 2 ,k+l) ) a2 + (xte(4 ,k+l) ) a2); 
qll = varv * (xte(2,k+l) / vt)A2 + varth * (xte( 4 ,k+l) ) a2; 
q22 = varv * (xte(4,k+l) / vt)A2 + varth * (xte(2,k+l))A2; 
ql2 = xte(2,k+l) * xte(4,k+l) * ((varv/vt)A2 - varth ); 
q21 = ql2; 

Q1 = [ qll ql 2 

q21 q22 1; 

Q = del*Ql*( del ' ) ; 

% PROJECT ERROR COVARIANCE 
P = phi*P*(phi') + Q; 

% UPDATE H 

deltx = xte(l,k+l); 

deity = xte(3,k+l); 

r2 = (deltx) 2 + (deity) 2; 

r = sqrt(r2); 

H = [ delty/r2 0 -deltx/r2 0 

deltx/r 0 delty/r 0 ] ; 
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% GET NEW MEASURED ESTIMATE 

delx = xte( 1 ,k+l); 

dely = xte(3,k+l); 

zhatl = atan2(delx,dely); 

zhat2 = sqrt(delxA2 + delyA2); 

zhat = [ zhatl 

zhat2 ] ; 

% COMPUTATION OF KALMAN GAIN 

vresid = (H*P*(H’) + R); % variance of the residual 

G = P*(H ! ) / vresid; % Kalman gain 

% COMPUTATION OF PROJECTED ERROR COVARIANCE 
P = (I - G*H)*P; 

% COMPUTATION OF RESIDUAL 
resid = zobs(: ,k+l) - zhat; 

% IF RESIDUAL TOO BIG, RESET P AND DO CALCULATIONS AGAIN, 
if ( abs( resid( 1) ) > 1. 0 ,v sqrt ( abs( vresid( 1 , 1 ) ) ) ... 

] abs( resid( 2) ) > 1. 0*sqrt(abs( vresid( 2, 2) ) ) ... 

& nbad < 2) 
nbad = nbad + 1; 
k = k-1; 

P = PO; % reset P 

else 

% COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+l) = xte(: ,k+l) + G*resid; 

% ERROR ELLIPSOID PLOTTING ROUTINE 
M = [xte(l,k+l) xte( 3 ,k+l)] 1 ; 

K = [ P(l,l) P( 1 , 3) 

P(3, 1) P(3,3) ] ; 

[xp,yp] = erre llip(M,K, . 60 , 25 , 0); 
eex( : ,k) = xp; 
eey( : ,k) = yp; 

% PREDICTED e FUTURE TRACK) USING KALMAN ESTIMATE 
% (number of predictions desired input from marine. m) 
tpred(: ,1) = xte(: ,k+l); 
acc = zeros(2, 1); 

for 1 = 1: kpred % kpred is # of predictions 

if k >= 5 

accx = (xte(2,k+l)-xte(2,k))/dt; 
accy = (xte(4,k+l)-xte(4,k))/dt; 
acc = [ accx; accy] ; 
if accx >= 5 
break 

acc = zeros( 2 , 1 ); 
elseif accy >= 5 
break 

acc = zeros( 2,1); 

end 

end 

tpred(: ,1+1) = phi*tpred(: ,l)+del*acc; 
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end 

nbad = 0; 

end 



% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
plot(xfol( 1 , 1: k+1) ,xfol( 3 , 1: k+1) , 'xw' ,xfo2( 1 , 1: k+1) , 
xfo2(3,l: k+1) ,'xw' . . 

xto( 1 , 1: k+1 ) ,xto( 3 , 1: k+1) , ' . b' ,xte( 1 , 1: k+1) ,xte( 3 , 1: k+1) , '+w' 
zpos( 1 , 1: k+1) ,zpos( 2 , 1: k+1) , ' ow' , tpred( 1, : ) ,tpred(3, : ) , '*g' , . 
eex.eey, ' -r' ) ,grid 

title( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS 
USING BRG/RNG OBS'),. . . 

xlabelC 'DISTANCE X' ) ,ylabel( ’DISTANCE Y') 



end 

diary off 
meta brgrng 



°L JOHN A. HUCKS II 
% FILE NAMF * DOPPLER M 

% EXTENDED KALMAN FILTER PROGRAM USING A DOPPLER SHIFTED FREQUENCY 
% SIGNAL AND THE BEARING TO THAT SIGNAL. 

% THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 
% FILTER, WITH INPUTS FROM A FUSED SENSOR ARRAY AS IT TRACKS A TARGET 
% ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH PROJECTION). 



clg 

clear eex eey xto 
! erase doppler.met 
diary doppler 

% STATE EQUATIONS 



% OBSERVATION TIME INTERVAL (input from marine, m) 

% STATE TRANSITION MATRIX 
phi = [ 1 dt 0 

0 
1 



0 

0 

0 

0 



1 

0 

0 0 
0 0 



0 

0 



0 

0 



dt 0 



1 

0 



0 

1 ]; 



% SYSTEM NOISE COEFFICIENT MATRIX 



del = 



[ dt 
0 
0 
0 
0 



dt*dt/2 

dt 

0 

0 

0 



0 

0 

dt 

0 

0 



0 

0 

dt*dt/2 

dt 

0 



0 

0 

0 

0 

dt ]; 



% STATE VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 



% NUMBER OF OBSERVATIONS (input from marine. m) 
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% ESTIMATED TARGET STATE 
xte = zeros(5 ,kmax); 



% initial est. target state 



% OBSERVER (SENSOR) INITIAL POSITIONS AND VELOCITIES 

% (input from marine, m) 

xfoa = zeros(4 ,kmax); 

xfoa(: ,1) = xfol(: ,1); 

xfol = xfoa; 



% OBSERVER SPEED ( input from marine, m) 

% INITIAL TIME SETTING 
Time = zeros( 1 ,kmax); 



% ACTUAL TRACK POSITIONS (input from marine, m) 
x = zeros( 1 ,kmax); 
y = zeros( 1 ,kmax); 

T = 0; 
for k = 1 

x(k) = tgt( 1,1); 
y(k) = tgt( 4 , 1) ; 
fo(k) =tgt( 7,1); 

end 

for k = 2: kmax 

T = T+dt; 

x(k) = ( 0 . 5*tgt ( 3 , 1)*(Ta2) )+( tgt( 2 , 1) . *T)+( tgt( 1,1)); 
vx(k) = ( tgt( 3 , l)*T)+tgt( 2,1); 

y(k) = ( 0. 5*tgt( 6 , 1 )*(Ta 2) )+( tgt( 5,1). *T)+( tgt(4, 1) ); 
vy(k) = ( tgt( 6 , l)*T)+tgt( 5,1); 
fo(k) = tgt( 7,1); 

end 

zpos = [ X 
vx 

y 

vy 

fo ] ; % rest frequency xmtr (Hz) 



o/ 

Jo 

Of 

Jo 



INPUT OBSERVATION VALUES (BEARING/FREQUENCY) 
INITIAL OBSERVED TARGET STATE 



xto = zeros(5 ,kmax); 
xto(: ,1) = [ tgt(l,l) 



(km in x direction) 
(km/hr in x direction) 
(km in y direction) 
(km/hr in y direction) 
rest frequency xmtr (Hz) 



tgt(2,l) 

tgt(4,l) 
tgt(5,l) 
tgt(7,l) ]; 

deltlx = zpos(l,: ) - xfol(l,: ); 
deltly = zpos(3 3 :) - xfol(3,:); 
brgl = atan2(deltlx, deltly); % 

vp = 1. 0789e9; % 

for k = 1: kmax 

f (k) = ( zpos( 5 ,k)' v vp)/( vp+( (zpos( 1 ,k)*zpos( 2 ,k) )+(zpos( 3 ,k)*. 
zpos (4 ,k) ) )/sqrt( ( zpos( 1 , k) A2+zpos( 3 , k) a2) ) ) ; 



bearing thtal 
spd light (km/hr) 



end 

zobs = [ brgl; f] ; 



xtol = zeros( 5 ,kmax) ; 
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xtol(l,:) = zpos(l,:); 
xtol(2,:) = zpos(2,:); 
xtol(3,: ) = zpos(3,: ); 
xtol(4,:) = zpos(4,:); 
xtol(5,:) = zpos(5,:); 
xtol( : , 1 ) = xto( : , 1) ; 
xto = xtol; 



% STATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 
varv = 1. 005; % variance of linear acceleration 

varth = 5. 05; % variance of angular acceleration 

vfreq =1.0; % variance of the rest frequency 



R = [ le-11 0 

0 5e- 11 ]; 



% measurement noise 



% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ' normal 1 ) 

zobs = zobs + sqrt(R( 1, l))*rand(zobs); 



% INITIAL KALMAN MATRICES AND OBSERVATION 
I = eye(5); 

% INITIAL KALMAN P MATRIX 

P0 = [ 300 0000 % reset value of Error Covariance 

0 300 0 00 

0 0 300 00 

0 00 300 0 

0 00 0 300 ] ; 

% REINITIALIZING P MATRIX 
P = P0; 

% KALMAN ESTIMATE 



xte( : , 1) = [ xto( 1,1) 


O 

/o 


(km - x direction) 


tgt( 2,1) 


O/ 

/o 


(km/hr) 


xto( 3 , 1 ) 


°/o 


(km - y direction) 


tgt(5 , 1) 


O ! 

/o 


(km/hr) 


tgt(7,l) ]; 


% 


rest frequency xmtr (Hz)_ 



tpred = xte( : , 1); 



% SIMULATION OF KALMAN FILTER OPERATION 
% NUMBER OF BAD OBSERVATIONS 

nbad =0; % initial setting bad obs. cntr. 

% SIMULATION COUNTER LOOP 

k = 0; 

for kl=l: 10*kmax 

k = k + 1; % actual loop counter 

if (k = kmax) % check for termination 

break 

end 

% UPDATE FORWARD OBSERVER'S/ FAC’S POSITION (IF THEY ARE MOVING) 
if k <= 3 
way = pi/2; 
else 
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way = -(3*pi/4); 

end 

if spd = 0 

xfol( 1 ,k+l) = xfol( 1 ,k); 
xfol(3,k+l) = xfol(3,k); 
else 

xfol(l,k+l) = xfol(l,k) + spd*sin(way)*dt; 
xfol(2,k+l) = spd*sin( way) ; 
xfol(3,k+l) = xfol(3,k) + spd*cos(way)*dt; 
xfol(4,k+l) = spd*cos(way); 

end 

% PROJECT AHEAD KALMAN STATE ESTIMATE 
xte(: ,k+l) = phi*xte(: ,k); 

% UPDATE Q 

vt = sqrt((xte(2,k+l))A2 + (xte(4,k+l) ) a2); 

sigxdd2 = ((xte(2,k+l)/vt)A2*varv) + ((xte(4,k+l) A2)*varth); 

sigydd2 = ( (xte( 4 ,k+l) /vt ) a 2 ,v varv) + ( (xte( 2 ,k+l) A2)*varth) ; 

sigxydd = (xte( 2 ,k+l)*xte(4,k+l) )*( (varv/(vt*vt) ) -varth); 

qll = ( ( dt ,v dt )/2) A2*sigxdd2; 

ql2 = ( (dt*dt*dt) /2)’ ! '-sigxdd2‘, 

ql3 = ((dt*dt)/2) ,v sigxydd; 

ql4 = ( (dt*dt*dt)/2)' v sigxydd; 

ql5 = 0; 

q21 = q 1 2 ; 

q22 = ( dt*dt)*sigxdd2; 

q23 = ( (dt*dt*dt)/2)*sigxydd; 

q24 = ( dt*dt)*sigxydd; 

q25 = 0; 

q31 = q 1 3; 

q32 = q23; 

q33 = ( ( dt*dt ) /2) A2*sigvdd2; 

q34 = ((dt*dt*dt)/2)*sigydd2; 

q35 = 0; 

q41 = ql4; 

q42 = q24; 

q43 = q34; 

q44 = (dt*dt)*sigydd2; 

q45 = 0; 

q51 = ql5; 

q52 = q25; 

q53 = q35; 

q54 = q45; 

q55 = (dt*dt)*vfreq; 



Q1 = [ qll ql2 ql3 ql4 ql5 
q2 1 q22 q23 q24 q25 
q31 q32 q33 q34 q35 
q41 q42 q43 q44 q45 
q5 1 q52 q53 q54 q 55 ]; 



Q = del*Ql*(del 1 ); 

% PROJECT ERROR COVARIANCE 
P = phi*P*(phi ' ) + Q; 
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% UPDATE H 

rl2 = sqrt( (xte( 1 ,k+l) A2)+(xte( 3,k+l) a 2) ); 
r32 = (sqrt( (xte( 1 ,k+l) A2)+(xte( 3,k+l) a2) ) ) a 3; 
deltxvy = xte( l,k+l)*xte(4,k+l) - xte(3,k+l)*xte(2,k+l); 
deltyvx = xte( 3 ,k+l)*xte( 2 ,k+l) - xte( l,k+l)*xte(4,k+l); 
fkvp = xte(5 ,k+l)*vp; 

hll = -(xte(3,k+l)/((xte(l,k+l)A 2)+(xte(3 ,k+D *2) ) ); 
hl2 = 0; 

hl3 = xte( 1 ,k+l)/( (xte( 1 ,k+l) A2)+(xte(3,k+l) a 2) ); 
hl4 = 0; 
hl5 = 0; 

h21 = -((zobs(2,k+l)A2)/fkvp)*(xte(3,k+l)*deltyvx)/r32; 
h22 = -( (zobs( 2 ,k+l) A2)/fkvp)*(xte( 1 ,k+l)/rl2); 
h23 = -(zobs(2 ,k+l)/fkvp)*(xte( 1 ,k+l)*deltxvy)/r32; 
h24 = -((zobs(2,k+l)A2)/fkvp)*(xte(3,k+l)/rl2); 
h25 = zobs(2,k+l)/xte(5,k+l); 

H = [ hll hl2 hl3 hl4 hl5 

h21 h22 h23 h24 h25 ]; 

% GET NEW MEASURED ESTIMATE 
deltlx = xte(l,k+l) - xfol(l,k+l); 
deltlv = xte(3,k+l) - xfol(3,k+l); 

zhatl = atan2(deltlx,deltly); % bearing estimate thtal 

zhat2 = (xte(5 ,k+l)*vp)/(vp+((xte( l,k+l)* % frequency estimate 

xte(2,k+l))+(xte(3,k+l)*. . . 

xte(4,k+l)))/sqrt((xte(l ,k+l) A2+xte( 3,k+l) a 2) ) ) ; 
zhat = [ zhatl 

zhat2 ] ; 

% COMPUTATION OF KALMAN GAIN 

vresid = ( H‘ ; 'P*( H ' ) + R); X variance of the residual 

G = P*(H') / vresid; X Kalman gain 

X COMPUTATION OF UPDATED ERROR COVARIANCE 
P = (I - G*H)*P; 

% COMPUTATION OF RESIDUAL 
resid = zobs(: ,k+l) - zhat; 

X IF RESIDUAL IS TOO BIG, THEN RESET P AND DO CALCULATIONS AGAIN, 
if ( abs( resid( 1) ) > 1. 0*sqrt( abs( vresid( 1 , 1) ) ) ... 

] abs(resid(2)) > 1. 0*sqrt( abs( vresid( 2 , 2) ) ) ... 

& nbad == 0) 
nbad - 1; 
k = k-1; 

P = PO; X reset P 

else 

% COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+l) = xte(: ,k+l) + G*resid; 

X ERROR ELLIPSOID PLOTTING ROUTINE 
M = [ xte( l,k+l) xte(3,k+l)] ; 

K = [ P(l,l) P(l, 3) 
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$s a* as a s as as as as as as as 



P( 3 , 1) P( 3 , 3) ]; 

[xp,yp] = errellip(M,K,. 60,25,0); 
eex( : ,k) = xp; 
eey(: ,k) = yp; 

% PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
tpred(: ,1) = xte(: ,k+l); 
acc = zeros(5, 1); 
for 1 = 1: kmax 
if k >= 3 

acc( 2 , 1) = (xte(2,k+l)-xte(2,k))/dt; 
acc(4,l) = (xte(4,k+l)-xte(4,k))/dt; 
if acc( 2,1) >= 5 
break 

acc = zeros(5 , 1); 
elseif acc(4,l) >= 5 
break 

acc = zeros(5,l); 

end 

end 

tpred(: ,1+1) = phi*tpred(: , l)+del*acc; 

end 

nbad = 0; 
end 

% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
plot(xfol(l,l: k+1) ,xfol( 3 , 1: k+1) , ' xw' ,xto( 1 , 1: k+1) , 
xto( 3 , 1: k+1) , ' . b' , . . . 

xte( 1 , 1: k+1 ) ,xte( 3 , 1: k+1) ^ ' +w' , zpos( 1 , 1: k+1) , zpos( 3 , 1: k+1) , ' ow* , . 
tpred( 1,: ) ,tpred(3,: ) , ' Vr g ,eex,eey, ' -r* ) ,grid 
tit le( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS 
USING DOPPLER ’),... 

xlabel( 'DISTANCE X' ) ,ylabel( ' DISTANCE Y' ) 

end 

meta doppler 



function [ x 3 y] = errellip(M,K,Pr ,n,MD) 

o/ 

% errellip(M,K,Pr ,n,MD) 

O, 

/o 

This function takes the following arguments: 

M Mean Vector (2x1) 

K Covariance Matrix (2x2) 

Pr Probability (0 .. 1) 

n Number of points to compute 

MD Compute by Mahalanobis Distance vice probability 

0 = Probability 

1 = Mahalanobis Distance 

and returns x and y vectors of the confidence ellipse for the 
given probability or Mahalanobis Distance. 

% Stephen L. Spehn 15 Nov 1989 
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if nargin = 5 

error( ' Incorrect number of arguments to errellip. 
end; 

if MD = 1 

c = abs(Pr); 
else 

Pr = max(min( . 995 , Pr) , . 005); 



% Cubic spline fit 
% Using this method 
pp = [ 12; . 005; . 01 
.95; .975; .99 
4. 190809 197869072e+l 

3. 970929262330003; 

1. 6288224111 1034e+l; 

4. 176985755223079e+2 
-3. 810356328008524e- 
5. 444089 169 224 142e-l 
2. 55841940780766; 

4. 607 04229 25247e+2; 

2. 020857475864537; 

2. 122852230998054; 

3. 862381141104123; 

4. 3821332 65 89 86 73 e+1 
.01; .0201; .0506; . 
1.39; 2.77; 4.61; 5. 
c = ppval(pp,Pr); 



for the ellipse constant. 

is a compromise between 
; .025; .05; .1; .25; .5; 
; . 995; 4. 0; 

; 4. 190809 197872 625e+l; 

2. 15930349191602; 

8. 243 758565 140 19e+l; 

; 2. 087990965023806e+5; 

1; 2. 475857468793011e-l; 

; 1. 140048306271903; 

1. 477458749113522e+l; 
4. 9 20 3 162 24 166436 e+2; 
2. 02019022643493; 

2. 20707509215777; 

8. 195632865839841; 

; 6. 763972895071458e+l; 

103; .211; .575; 

99; 7. 38; 9. 21 ] ; 



speed and accuracy. 
.75; .9; 

-2. 118721291999464e+l; 
5. 955793735647319e-l; 
2. 725551521454689e+3; 
2. 087990965023842e+5; 
2. 133449885921905; 

2. 111734877634124; 

5. 18 7 150 1034265 8 9e+l; 
9. 8 8 79 9 09 65 023 75 7e+3; 
2. 055905760926949; 

2. 694842569743673; 

1. 8 192546 144650 04e+l; 
2. 23340067 7 62321 e+2; 



end; 



% Get Eigenvectors, Eigenvalues, and translated variances 
[Evec,Eval] = eig(K); 
sigx = sqrt(Eval( 1 , 1) ); 
sigy = sqrt(Eval( 2 , 2) ); 

% Parameterized ellipse equations 
t = 0: ( 2*pi/n): (2*pi); 
xv = sigx*c*cos( t); 
yv = sigy*c*sin(t); 

% Translate back to the original coordinates 
x = (xv*Evec( 1 , 1) + yv*Evec(l,2) + M(l))'; 
y = (xv*Evec( 2,1) + yv*Evec(2,2) + M(2))'; 



function y = reshape(x ,m,n) 

^RESHAPE RESHAPE(X,M,N) returns the M-by-N matrix whose elements 
%are taken columnwise from X. An error results if X does 
%not have M*N elements. 
mm,nn = size(x); 
if mm*nn = m*n 

error( ' Matrix must have M*N elements. ') 
end 
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zeros(m,n); 



LIST OF REFERENCES 



1. Sorenson, H.W., "Kalman Filtering Techniques", Advanced Control Systems, Vol. 
3, Chap. 5, 1966. 

2. Tzu, Sun, The Art of J Tar, Clavell, J., ed., Delacorte Press, NY, 1983. 

3. 

U.S. Army Communications and Electronics Command, Contract No. 
DAAB07-83-C-J031. PLRS System Technical Description, Fort Monmouth, NJ, 
April 19S6. 

4. Mitschang. G.W., "An Application ofNonlinear Filtering Theory to Passive Target 
Location and Tracking", Doctoral Dissertation, Naval Postgraduate School, 
Monterey, CA. June 1974. 

5. Collin, R.E. Antennas and Radio Ware Propagation, p. 307, McGraw-Hill, NY, 

1985. 

6. Gelb, A., and others. Applied Optimal Estimation, Gelb, A., ed., p. 182, MIT Press, 
MA, 1974. 

7. Therrien, C.W., Decision Estimation and Classification, John Wiley and Sons, Inc., 
NY, 1989. 

8. Baheti. R.S., "Efficient Approximation of Kalman Filter for Target Tracking", 
IEEE Trans, on Aerospace and Electronic Systems, Vol. AES-22, No. 1, pp. 8-14, 

1986. 



82 



INITIAL DISTRIBUTION LIST 

No. Copies 

1. Defense Technical Information Center 2 

Cameron Station 

Alexandria, VA 22304-6145 

2. Library, Code 0142 2 

Naval Postgraduate School 

Monterey, CA 93943-5002 

3. Commandant of the Marine Corps 1 

Code TE 06 

Headquarters, U. S. Marine Corps 
Washington, D.C. 20380-0001 

4. Chairman. Code 62 1 

Department of Electrical and Computer Engineering 

Naval Postgraduate School 
Monterey, CA 93943-5000 

5. Professor Harold A. Titus. Code 62Ts 2 

Department of Electrical and Computer Engineering 

Naval Postgraduate School 
Monterey, CA 93943-5000 

6. Professor Roberto Cristi. Code 62Cx 1 

Department of Electrical and Computer Engineering 

Naval Postgraduate School 
Monterey, CA 93943-5000 

7. Professor Mostafa Ghandahari, Code 53Gh 1 

Department of Mathematics 

Naval Postgraduate School 
Monterey, CA 93943-5000 

8. Commanding Officer 5 

Attn: CAPTJ.A. Hucks II US.MC 

Marine Corps Tactical Systems Support Activity 

Marine Corps Base 

Camp Pendleton, CA 92055-6045 



83 



(_/ *£'(3 



Thesis 
H843 
c. 1 



Hucks 

Fusion of ground-based 
sensors for optimal 
tracking of military tar- 
gets. 



